点云_四元数和旋转矩阵之间互相转换

点云

旋转矩阵  四元数 欧拉角

旋转矩阵转换为四元数

 {  "Array": [
        -0.726405369155686,
        0.6872664061155301,
        0.000355930941022522,
        6855.053677575546,
        -0.6872643549187422,
        -0.7264042997375078,
        0.0021212687208797248,
        -19706.759414176922,
        0.0017164264961726733,
        0.0012962823396915819,
        0.9999976867634139,
        13.437930198386312,
        0.0,
        0.0,
        0.0,
        1.0],
 "Quaternion": [
    {  "w": 0.3698607906598847,
        "x": -0.0005576330351997091,
        "y": -0.0009195997450303073,
        "z": -0.9290865615830163,
        "pose_x": 6855.053677575546,
        "pose_y": -19706.759414176922,
        "pose_z": 13.437930198386312 }] }

代码示例01

from scipy.spatial.transform import Rotation as R
import numpy as np
if __name__ == '__main__':
    # "x","y" ,"z" ,"w"
    Rquater= [-0.0005576330351997091, -0.0009195997450303073, -0.9290865615830163, 0.3698607906598847]
    # 四元数到旋转矩阵
    rotate_spat = R.from_quat(Rquater)
    rotate_mat = rotate_spat.as_matrix()
    print(rotate_mat)
    
    rotate_matrix_list = [
     [-0.726405369155686,0.6872664061155301, 0.000355930941022522],
     [ -0.6872643549187422, -0.7264042997375078,0.0021212687208797248],
     [ 0.0017164264961726733, 0.0012962823396915819,0.9999976867634139]]
    rotate_matrix = np.array(rotate_matrix_list)
    # 旋转矩阵到四元数
    r3 = R.from_matrix(rotate_matrix)
    qua = r3.as_quat()
    print(qua)

代码示例02

from pyquaternion import Quaternion
import numpy as np

if __name__ == '__main__':
    rotate_matrix_list = [-0.726405369155686,0.6872664061155301, 0.000355930941022522],
     [ -0.6872643549187422, -0.7264042997375078,0.0021212687208797248],
     [ 0.0017164264961726733, 0.0012962823396915819,0.9999976867634139]]
    rotate_array = np.array(rotate_matrix_list)
    quater = Quaternion(matrix=rotate_array)
    print(quater)  
    print(f"x: {quater.x}, y: {quater.y}, z: {quater.z}, w: {quater.w}")    

说明

函数np.allclose(a, b)比较具有给定精确度范围内的浮点数组

附录

https://blog.csdn.net/shyjhyp11/article/details/111701127
http://kieranwynn.github.io/pyquaternion/
https://github.com/KieranWynn/pyquaternion