对相机位姿 导出 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)
相关推荐
weiwuxian几秒前
揭开智能体的神秘面纱:原来你不是"超级AI"!
人工智能
Codebee1 分钟前
“自举开发“范式:OneCode如何用低代码重构自身工具链
java·人工智能·架构
说私域12 分钟前
基于开源AI智能名片链动2+1模式的S2B2C商城小程序:门店私域流量与视频号直播融合的生态创新研究
人工智能·小程序·开源
Ronin-Lotus15 分钟前
深度学习篇---Yolov系列
人工智能·深度学习
静心问道43 分钟前
GoT:超越思维链:语言模型中的有效思维图推理
人工智能·计算机视觉·语言模型
aneasystone本尊1 小时前
学习 Claude Code 的工具使用(三)
人工智能
szxinmai主板定制专家1 小时前
【精密测量】基于ARM+FPGA的多路光栅信号采集方案
服务器·arm开发·人工智能·嵌入式硬件·fpga开发
T__TIII1 小时前
Dify 自定义插件
人工智能·github
快起来别睡了2 小时前
LangChain 介绍及使用指南:从“会聊天”到“能干活”的 AI 应用开发工具
人工智能
AI数据皮皮侠2 小时前
中国区域10m空间分辨率楼高数据集(全国/分省/分市/免费数据)
大数据·人工智能·机器学习·分类·业界资讯