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

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

四元数 -> 旋转矩阵

方法1

使用公式: https://doc.rc-visard.com/latest/de/pose_formats.html?highlight=format

 

方法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/

# 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])