python_四元数q转旋转矩阵R(已验证)

水深无声 2022-12-30 10:36 174阅读 0赞

注意: **符号相反**的四元数,代表同一个旋转 (q = -q )

# 四元数 -> 旋转矩阵 #

## 方法1 ##

使用公式: [https://doc.rc-visard.com/latest/de/pose\_formats.html?highlight=format][https_doc.rc-visard.com_latest_de_pose_formats.html_highlight_format]

![20201224203021833.png][]

## 方法2 ##

使用库scipy中的方法 Rm = R.from\_quat(Rq)

from scipy.spatial.transform import Rotation as R
    
    Rq=[-0.35, 1.23e-06, 4.18e-08, 0.39]
    Rm = R.from_quat(Rq)
    rotation_matrix = Rm.as_matrix()
    print('rotation:\n',rotation_matrix)
    
    #结果:
    rotation:
     [[ 1.00000000e+00 -3.25420248e-06  3.38725419e-06]
     [-3.01673707e-06  1.07793154e-01  9.94173343e-01]
     [-3.60036417e-06 -9.94173343e-01  1.07793154e-01]]

备注: SLAM十四讲 第一版中公式(3.35)形式是对的,但R的下标和位置有误, 在第二版中未再提及该公式

\-----------------------

下面两个转换待验证

# 旋转矩阵-> 欧拉角 #

参考:[https://www.learnopencv.com/rotation-matrix-to-euler-angles/][https_www.learnopencv.com_rotation-matrix-to-euler-angles]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70][]

# Calculates Rotation Matrix given euler angles.
    def eulerAnglesToRotationMatrix(theta) :
        
        R_x = np.array([[1,         0,                  0                   ],
                        [0,         math.cos(theta[0]), -math.sin(theta[0]) ],
                        [0,         math.sin(theta[0]), math.cos(theta[0])  ]
                        ])
                        
        R_y = np.array([[math.cos(theta[1]),    0,      math.sin(theta[1])  ],
                        [0,                     1,      0                   ],
                        [-math.sin(theta[1]),   0,      math.cos(theta[1])  ]
                        ])
                    
        R_z = np.array([[math.cos(theta[2]),    -math.sin(theta[2]),    0],
                        [math.sin(theta[2]),    math.cos(theta[2]),     0],
                        [0,                     0,                      1]
                        ])               
                        
        R = np.dot(R_z, np.dot( R_y, R_x ))
    
        return R

# 欧拉角-> 旋转矩阵 #

# Checks if a matrix is a valid rotation matrix.
    def isRotationMatrix(R) :
        Rt = np.transpose(R)
        shouldBeIdentity = np.dot(Rt, R)
        I = np.identity(3, dtype = R.dtype)
        n = np.linalg.norm(I - shouldBeIdentity)
        return n < 1e-6
    
    # Calculates rotation matrix to euler angles
    # The result is the same as MATLAB except the order
    # of the euler angles ( x and z are swapped ).
    def rotationMatrixToEulerAngles(R) :
    
        assert(isRotationMatrix(R)) 
        sy = math.sqrt(R[0,0] * R[0,0] +  R[1,0] * R[1,0]) 
        singular = sy < 1e-6
    
        if  not singular :
            x = math.atan2(R[2,1] , R[2,2])
            y = math.atan2(-R[2,0], sy)
            z = math.atan2(R[1,0], R[0,0])
        else :
            x = math.atan2(-R[1,2], R[1,1])
            y = math.atan2(-R[2,0], sy)
            z = 0
    
        return np.array([x, y, z])

[https_doc.rc-visard.com_latest_de_pose_formats.html_highlight_format]: https://doc.rc-visard.com/latest/de/pose_formats.html?highlight=format
[20201224203021833.png]: /images/20221120/d30604bf7a5a4f4e9807299c0b7352e5.png
[https_www.learnopencv.com_rotation-matrix-to-euler-angles]: https://www.learnopencv.com/rotation-matrix-to-euler-angles/
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3NoeWpoeXAxMQ_size_16_color_FFFFFF_t_70]: /images/20221120/828487b3f97b46f1b013ca76d46df2e5.png