1 python实现旋转矩阵转换为四元数

例如：下面把3x3的旋转矩阵转换为四元数

from pyquaternion import Quaternion


rotate_matrix = [[-0.0174524064372832, -0.999847695156391, 0.0],
                 [0.308969929589947, -0.00539309018185907, -0.951056516295153],
                 [0.950911665781176, -0.0165982248672099, 0.309016994374948]]

RM = np.array(rotate_matrix)

# 旋转矩阵转换为四元数
def rotateToQuaternion(rotateMatrix):
    q = Quaternion(matrix=rotateMatrix)
    print(q)  # 0.567 +0.412i -0.419j +0.577k
    print(f"x: {q.x}, y: {q.y}, z: {q.z}, w: {q.w}")
    # x: 0.41198412875061946, y: -0.41923809520381, z: 0.5770317346112972, w: 0.567047506333421
    return q


if __name__ == '__main__':
    rotateMatrixToEulerAngles(RM)
    rotateMatrixToEulerAngles2(RM)

    euler_angles = [-0.05366141770874149, -1.2561686529408898, 1.6272221428848495]
    eulerAnglesToRotationMatrix(euler_angles)

    rotateToQuaternion(RM)

输出结果如下：

0.567 +0.412i -0.419j +0.577k
x: 0.41198412875061946, y: -0.41923809520381, z: 0.5770317346112972, w: 0.567047506333421