python 四元数、旋转矩阵、欧拉角的相互转化

from scipy.spatial.transform import Rotation

这库里都写好了

import numpy as np
import xml.etree.ElementTree as ET
from scipy.spatial.transform import Rotation


def ReadXml_RT_Data(filename):


    tree = ET.parse(filename)
    root = tree.getroot()

    rotation_elem = root.find('rotation')
    type_id = rotation_elem.attrib['type_id']
    rows = int(rotation_elem.find('rows').text)
    cols = int(rotation_elem.find('cols').text)
    dt = rotation_elem.find('dt').text
    rot_data = rotation_elem.find('data').text.split()

    # print(f'Type ID: {type_id}')
    # print(f'Rows: {rows}')
    # print(f'Cols: {cols}')
    # print(f'Data type: {dt}')
    # print(f'Data: {data}')

    trans_elem = root.find('translation')
    trans_data = trans_elem.find('data').text.split()

    return rot_data, trans_data


# 读取xml文件中旋转矩阵值
Mat_Cam2Pro, Mat_Cam2Pro_trans = ReadXml_RT_Data("CamPro.xml")
# print(Mat_Cam2Pro)
# print(Mat_Cam2Pro[0])
print(Mat_Cam2Pro_trans)


# 定义旋转矩阵
_rot_matrix = np.array([[float(Mat_Cam2Pro[0]), float(Mat_Cam2Pro[1]), float(Mat_Cam2Pro[2])],
                        [float(Mat_Cam2Pro[3]), float(Mat_Cam2Pro[4]), float(Mat_Cam2Pro[5])],
                        [float(Mat_Cam2Pro[6]), float(Mat_Cam2Pro[7]), float(Mat_Cam2Pro[8])]])
# print("_rot_matrix", _rot_matrix)
# 从A相对于B转化为B相对于A的旋转矩阵

# 将上述右手坐标系的值转化为左手坐标系中

# Left_rot_matrix = np.array([[float(Mat_Cam2Pro[0]), float(Mat_Cam2Pro[1]), -float(Mat_Cam2Pro[2])],
#                        [float(Mat_Cam2Pro[3]), float(Mat_Cam2Pro[4]), -float(Mat_Cam2Pro[5])],
#                        [-float(Mat_Cam2Pro[6]), -float(Mat_Cam2Pro[7]), float(Mat_Cam2Pro[8])]])

# rot_matrix = np.linalg.inv(_rot_matrix)


R_dcm = _rot_matrix.reshape(3,3)
# print("R_dcm",R_dcm)
# 将旋转矩阵转化为四元数
R_quat = Rotation.from_matrix(R_dcm).as_quat()
# 将旋转矩阵转化为欧拉角
test_euler = Rotation.from_matrix(R_dcm).as_euler("xyz",degrees=True)
# print("四元数",R_quat)
print("euler",test_euler)

# # 将欧拉角转化为四元数
# test_qua = Rotation.from_euler('xyz', [62.207, 2.079, 124.779], degrees=True).as_quat()
#
# # 将欧拉角转化为旋转矩阵
# # test_qua = Rotation.from_euler('xyz', [62.207, 2.079, 124.779], degrees=True).as_matrix()
# # 这个矩阵是在左手坐标系中的
# print("test_qua", test_qua)

# # 从A相对于B转化为B相对于A的平移
# R_inv = rot_matrix
# matrix_T = [[float(Mat_Cam2Pro_trans[0])],[float(Mat_Cam2Pro_trans[1])],[float(Mat_Cam2Pro_trans[2])]]
#
# # 将毫米转化为米
# trans = -np.dot(R_inv, matrix_T)*0.001
# # print("平移：", trans[0])
#
# print('平移: {:.5f}, {:.5f}, {:.5f}'.format(trans[0][0], trans[1][0], trans[2][0]))
#
# # 计算欧拉角
# # test_euler = Rotation.from_matrix(R_dcm).as_euler("xyz",degrees=True)
# # 下面计算部分可以通过上面一句来计算
# 如需将弧度转换为角度，可以使用np.rad2deg()函数。
# 请注意此代码仅适用于旋转矩阵中绕z、y、x轴依次进行旋转的情形，
# 若旋转顺序不同，请对代码进行相应修改。
# sy = np.sqrt(rot_matrix[0, 0] * rot_matrix[0, 0] + rot_matrix[1, 0] * rot_matrix[1, 0])
# if sy > 1e-6:
#     x = np.arctan2(rot_matrix[2, 1], rot_matrix[2, 2])
#     y = np.arctan2(-rot_matrix[2, 0], sy)
#     z = np.arctan2(rot_matrix[1, 0], rot_matrix[0, 0])
# else:
#     x = np.arctan2(-rot_matrix[1, 2], rot_matrix[1, 1])
#     y = np.arctan2(-rot_matrix[2, 0], sy)
#     z = 0
#
#
# # 打印结果
# # print('欧拉角 (rad弧度): {:.3f}, {:.3f}, {:.3f}'.format(x, y, z))
# # 弧度转化为角度
# x = np.rad2deg(x)
# y = np.rad2deg(y)
# z = np.rad2deg(z)
#
# # 打印结果
# print('欧拉角 (角度): {:.3f}, {:.3f}, {:.3f}'.format(x, y, z))

python 四元数、旋转矩阵、欧拉角的相互转化

猜你喜欢