（slam工具）6 python四元数转化旋转矩阵

import numpy as np
from scipy.spatial.transform import Rotation as R
import pyproj
from pyproj import Proj, transform

#0.016938035523210708 0.58455146147157355 -0.48870579156409283 0.64744060819180593 -129342.74756339534 3469822.8668770161 5343696.2087743161
# 四元数 (QW, QX, QY, QZ)
qw, qx, qy, qz = 0.016938035523210708, 0.58455146147157355, -0.48870579156409283, 0.64744060819180593   # 替换为实际值

# 平移向量 (TX, TY, TZ)
tx, ty, tz = -129342.74756339534, 3469822.8668770161, 5343696.2087743161

# 将四元数转换为旋转矩阵
rotation = R.from_quat([qx, qy, qz, qw])
rotation_matrix = rotation.as_matrix()

# 计算相机中心坐标(ECEF坐标系下)
camera_center = -np.dot(rotation_matrix.T, np.array([tx, ty, tz]))
print("投影中心坐标:（ecef坐标系下）", camera_center)

# 定义坐标转换
# 从ECEF转换到WGS84
ecef = Proj(proj='geocent', ellps='WGS84', datum='WGS84')
wgs84 = Proj(proj='latlong', ellps='WGS84', datum='WGS84')

# 执行转换
lon, lat, alt = transform(ecef, wgs84, camera_center[0],camera_center[1], camera_center[2], radians=False)

# 输出WGS84经纬度坐标
print("经度:", lon, "纬度:", lat, "高度:", alt)

# 从WGS84转换到UTM Zone 50N
utm_zone_50n = Proj(proj="utm", zone=50, ellps='WGS84', datum='WGS84', south=False)
utm_x, utm_y = transform(wgs84, utm_zone_50n, lon, lat)

# 输出UTM坐标
print("WGS84坐标系下：UTM Zone 50N X:", utm_x, "Y:", utm_y,"Z:",alt)

# 将四元数转换为旋转矩阵
rotation = R.from_quat([qx, qy, qz, qw])
rotation_matrix = rotation.as_matrix()

# 将旋转矩阵转换为欧拉角 (Omega, Phi, Kappa)
# 摄影测量中通常使用 ZYX 旋转顺序
omega, phi, kappa = rotation.as_euler('ZYX', degrees=True)
print("旋转矩阵:\n", rotation_matrix)

print("Omega:", omega, "Phi:", phi, "Kappa:", kappa)