SLAM_四元数取逆－Quaternion Inverse - 已验证

待我称王封你为后i 2023-01-22 04:55 135阅读 0赞

# 1 [四元数的一些运算][Link 1] #

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70][]

# 2 四元数取逆 #

对于单位四元数（四元素），其实就是xyz的值分别取反

q = \{x,y,z,w\}

q\_inverse = \{-x,-y,-z ,w\}

同时旋转矩阵取逆的方法如下：

// float mat[3][3] = { 00 01 02
    //                     10 11 12
    //                     20 21 22
    //                    }
    
    float m3_det2( float mat[3][3])
    {
        float det;
        det = mat[0][0] * ( mat[1][1]*mat[2][2] - mat[1][2]*mat[2][1] )
        - mat[1][0] * ( mat[0][1]*mat[2][2] - mat[0][2]*mat[2][1] )
        + mat[2][0] * ( mat[0][1]*mat[1][2] - mat[0][2]*mat[1][1] );
        return( det );
    }
     
    int m3_inverse2( float mr[3][3], float ma[3][3] )
    {
        float det = m3_det2( ma );
        if ( fabs( det ) < 0.0005 )
        {
            
            //        m3_identity( mr );
            return(0);
        }
        
        mr[0][0] =    ma[1][1]*ma[2][2] - ma[2][1]*ma[1][2]   / det;
        mr[1][0] = -( ma[1][0]*ma[2][2] - ma[1][2]*ma[2][0] ) / det;
        mr[2][0] =    ma[1][0]*ma[2][1] - ma[1][1]*ma[2][0]   / det;
        
        mr[0][1] = -( ma[0][1]*ma[2][2] - ma[2][1]*ma[0][2] ) / det;
        mr[1][1] =    ma[0][0]*ma[2][2] - ma[0][2]*ma[2][0]   / det;
        
        mr[2][1] = -( ma[0][0]*ma[2][1] - ma[0][1]*ma[2][0] ) / det;
        
        mr[0][2] =    ma[0][1]*ma[1][2] - ma[0][2]*ma[1][1]   / det;
        
        mr[1][2] = -( ma[0][0]*ma[1][2] - ma[0][2]*ma[1][0] ) / det;
        
        mr[2][2] =    ma[0][0]*ma[1][1] - ma[1][0]*ma[0][1]   / det;
        return(1);
    }

把上面的旋转矩阵取逆后再转换成四元素与四元素直接取逆得到的值一致。

[Link 1]: https://blog.csdn.net/silangquan/article/details/39008903
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70]: /images/20221020/6523062052d64973be65660df04280c4.png