需求:针对 [N,4,4] 格式的 poses np.darray 导出其 Tum 格式 的位姿。
时间戳根据 N 的值,线性得到。
python
import numpy as np
import os
import torch
from scipy.spatial.transform import Rotation
def rotation_matrix_to_tum_format(rotation_matrix):
rotation = Rotation.from_matrix(rotation_matrix)
quaternion = rotation.as_quat()
return quaternion
def convert_to_tum_format(poses, timestamps):
tum_poses = []
for i in range(poses.shape[0]):
pose = poses[i]
quaternion = rotation_matrix_to_tum_format(pose[:3, :3])
tum_timestamp = timestamps[i] * 0.1 # Scaling factor of 0.1 to convert timestamps to seconds
tum_pose = f"{tum_timestamp:.6f} {' '.join(map(str, pose[:3, 3]))} {' '.join(map(str, quaternion))}"
tum_poses.append(tum_pose)
return tum_poses
def write_tum_poses_to_file(file_path, tum_poses):
with open(file_path, 'w') as f:
for pose in tum_poses:
f.write(pose + '\n')
def convert_and_write_tum_poses(c2w_variable, output_filename, timestamps):
# 调用适当的函数将变量转换为 TUM 格式
tum_poses = convert_to_tum_format(c2w_variable, timestamps)
# 将 TUM 格式的位姿写入文件
write_tum_poses_to_file(output_filename, tum_poses)
n_poses = c2w_GT_traj.shape[0]
custom_timestamps = np.arange(n_poses)
convert_and_write_tum_poses(c2w_GT_traj, 'tum_c2w_GT_traj.txt', custom_timestamps)