python 用compose把json中的translation和rotation转换成{T，R}

#pip install pyquaternion
#pip install transforms3d 
import ast
from transforms3d.affines import compose
from transforms3d.quaternions import quat2mat
from pyquaternion import Quaternion

matched_pcd_pose_file = "matched_pcd_pose.txt"
matched_pcd_pose_list = []

with open(matched_pcd_pose_file) as f:
    for line in f:
        label = ast.literal_eval(line)

        ego_vehicle_to_world_rotation = \
        quat2mat([label.get('qw'), label.get('qx'), label.get('qy'), label.get('qz')])
		#Quaternion 也可以把四元数转orientation
		#ego_vehicle_to_world_rotation = \
		#Quaternion(np.array([q_w, q_x, q_y, q_z])).rotation_matrix
        ego_vehicle_to_world_translation = \
        [label.get('tx'), label.get('ty'), label.get('tz')]

        matched_pcd_pose_list.append(compose(ego_vehicle_to_world_translation,ego_vehicle_to_world_rotation,[1.0, 1.0, 1.0]))

print(matched_pcd_pose_list[1])

josn

{"tz": 27.544804, "qw": -0.09715, "tx": 641094.993213, "ty": 2764013.161824, "qy": 0.017784, "qx": 0.007416, "qz": -0.995083}

w, x, y, z = Quaternion(matrix) #可以直接从 R 获得 四元数

compose 可以用下面的函数替换掉

def merge_tr(translation, rotation):
    transform_matrix = np.zeros((4, 4), dtype=np.float32)
    transform_matrix[0:3, 0:3] = rotation
    transform_matrix[0:3, 3] = np.array([translation])
    transform_matrix[3, 0:4] = np.array([0, 0, 0, 1])
    return transform_matrix