对相机位姿 导出 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)
相关推荐
深蓝易网25 分钟前
为什么制造企业需要用MES管理系统升级改造车间
大数据·运维·人工智能·制造·devops
xiangzhihong833 分钟前
Amodal3R ,南洋理工推出的 3D 生成模型
人工智能·深度学习·计算机视觉
阿linlin40 分钟前
OpenCV--图像预处理学习01
opencv·学习·计算机视觉
狂奔solar1 小时前
diffusion-vas 提升遮挡区域的分割精度
人工智能·深度学习
资源大全免费分享1 小时前
MacOS 的 AI Agent 新星,本地沙盒驱动,解锁 macOS 操作新体验!
人工智能·macos·策略模式
跳跳糖炒酸奶1 小时前
第四章、Isaacsim在GUI中构建机器人(2):组装一个简单的机器人
人工智能·python·算法·ubuntu·机器人
AI.NET 极客圈1 小时前
AI与.NET技术实操系列(四):使用 Semantic Kernel 和 DeepSeek 构建AI应用
人工智能·.net
Debroon1 小时前
应华为 AI 医疗军团之战,各方动态和反应
人工智能·华为
俊哥V1 小时前
阿里通义千问发布全模态开源大模型Qwen2.5-Omni-7B
人工智能·ai
果冻人工智能2 小时前
每一条广告都只为你而生: 用 人工智能 颠覆广告行业的下一步
人工智能