import numpy as np
from scipy.spatial.transform import Rotation as R
import transformations as tf
# 手眼标定结果(示例数据)
x, y, z = 1.5, 2.0, 3.0
qx, qy, qz, qw = 0.7071, 0.0, 0.0, 0.7071
# 将四元数转换为旋转矩阵
rot_matrix = R.from_quat([qx, qy, qz, qw]).as_matrix()
# 构造变换矩阵
transform_matrix = np.eye(4)
transform_matrix[:3, :3] = rot_matrix
transform_matrix[:3, 3] = [x, y, z]
# 打印结果
print(transform_matrix)
其中,R.from_quat()
函数将四元数转换为旋转矩阵,np.eye(4)
函数创建一个4x4单位矩阵作为初始变换矩阵,transform_matrix[:3, :3]
用旋转矩阵替换掉初始变换矩阵的前三行前三列,transform_matrix[:3, 3]
用平移向量替换掉初始变换矩阵的前三行最后一列。