对相机位姿 导出 Tum 格式的位姿

需求:针对 [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)
相关推荐
kalvin_y_liu19 分钟前
智能体框架大PK!谷歌ADK VS 微软Semantic Kernel
人工智能·microsoft·谷歌·智能体
爱看科技23 分钟前
智能眼镜行业腾飞在即,苹果/微美全息锚定“AR+AI眼镜融合”之路抢滩市场!
人工智能·ar
星期天要睡觉1 小时前
计算机视觉(opencv)——基于 OpenCV DNN 的实时人脸检测 + 年龄与性别识别
opencv·计算机视觉·dnn
Juchecar3 小时前
LLM模型与ML算法之间的关系
人工智能
FIN66683 小时前
昂瑞微:深耕射频“芯”赛道以硬核实力冲刺科创板大门
前端·人工智能·科技·前端框架·信息与通信·智能
benben0443 小时前
京东agent之joyagent解读
人工智能
LONGZETECH3 小时前
【龙泽科技】汽车动力与驱动系统综合分析技术1+X仿真教学软件(1.1.3 -初级)
人工智能·科技·汽车·汽车仿真教学软件·汽车教学软件
lisw054 小时前
SolidWorks:现代工程设计与数字制造的核心平台
人工智能·机器学习·青少年编程·软件工程·制造
大刘讲IT4 小时前
AI 生产工艺参数优化:中小型制造企业用 “智能调参“ 提升产品合格率与生产效率
人工智能·制造
图欧学习资源库4 小时前
人工智能领域、图欧科技、IMYAI智能助手2025年9月更新月报
人工智能·科技