Python读取xml，旋转矩阵转化为欧拉角，A相对于B的旋转平移求B相对于A的旋转平移

CamPro.xml

<?xml version="1.0"?>
<opencv_storage>
<img_shape type_id="opencv-matrix">
  <rows>2</rows>
  <cols>1</cols>
  <dt>d</dt>
  <data>
    2160. 3840.</data></img_shape>
<rms>8.6848457031584418e+04</rms>
<cam_int type_id="opencv-matrix">
  <rows>3</rows>
  <cols>3</cols>
  <dt>d</dt>
  <data>
    2.8510834321129507e+04 0. 1.0020493890667072e+03 0.
    2.3030362629178228e+04 1.7192973327299374e+03 0. 0. 1.</data></cam_int>
<cam_dist type_id="opencv-matrix">
  <rows>1</rows>
  <cols>5</cols>
  <dt>d</dt>
  <data>
    3.0022474186130147e+01 -2.9749214145741257e+03
    -7.2531894219926021e-02 -8.6518733415383364e-01
    -7.9424743183410342e+00</data></cam_dist>
<proj_int type_id="opencv-matrix">
  <rows>3</rows>
  <cols>3</cols>
  <dt>d</dt>
  <data>
    6.3459244417384326e+01 0. 1.5546178717704811e+03 0.
    2.1596450999813856e+03 1.8640280874705297e+03 0. 0. 1.</data></proj_int>
<proj_dist type_id="opencv-matrix">
  <rows>1</rows>
  <cols>5</cols>
  <dt>d</dt>
  <data>
    3.9449273734638378e-02 1.0069782220407857e-03
    -6.4835064143111090e-02 -2.0584255676496738e-01
    8.0718092010593814e-05</data></proj_dist>
<rotation type_id="opencv-matrix">
  <rows>3</rows>
  <cols>3</cols>
  <dt>d</dt>
  <data>
    -5.7003538881072635e-01 8.2081906955671347e-01
    3.6272724676471912e-02 -4.0128921163876674e-01
    -2.3961677574168250e-01 -8.8405360097988228e-01
    -7.1695650086022722e-01 -5.1849769125350131e-01
    4.6597587924596195e-01</data></rotation>
<translation type_id="opencv-matrix">
  <rows>3</rows>
  <cols>1</cols>
  <dt>d</dt>
  <data>
    4.7357619261615682e+01 2.6159627983034142e+01 1.6008819544963842e+01</data></translation>
</opencv_storage>

pyhton

import numpy as np
import xml.etree.ElementTree as ET

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


# 如需将弧度转换为角度，可以使用np.rad2deg()函数。
# 请注意此代码仅适用于旋转矩阵中绕z、y、x轴依次进行旋转的情形，
# 若旋转顺序不同，请对代码进行相应修改。
# 定义旋转矩阵
_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_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)
# print("平移：", trans[0])
print('平移: {:.3f}, {:.3f}, {:.3f}'.format(trans[0][0], trans[1][0], trans[2][0]))

# 计算欧拉角
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))