对相机位姿 导出 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)
相关推荐
SelectDB4 分钟前
Apache Doris 4.0 AI 能力揭秘(一):AI 函数之 LLM 函数介绍
数据库·人工智能·数据分析
倔强青铜三5 分钟前
苦练Python第39天:海象操作符 := 的入门、实战与避坑指南
人工智能·python·面试
飞哥数智坊13 分钟前
GPT-5 初战:我用 Windsurf,体验了“结对编程”式的AI开发
人工智能·windsurf
数据超市37 分钟前
香港数据合集:建筑物、手机基站、POI、职住数据、用地类型
大数据·人工智能·智能手机·数据挖掘·数据分析
视觉语言导航1 小时前
哈工深无人机目标导航新基准!UAV-ON:开放世界空中智能体目标导向导航基准测试
人工智能·深度学习·无人机·具身智能
yzx9910131 小时前
AI心理助手开发文档
人工智能·深度学习·机器学习
图灵学术计算机论文辅导1 小时前
论文推荐|迁移学习+多模态特征融合
论文阅读·人工智能·深度学习·计算机网络·算法·计算机视觉·目标跟踪
CallZhang2101 小时前
Vision Master的C#脚本与opencv联合编程
opencv·计算机视觉·c#·视觉检测
一百天成为python专家1 小时前
Python循环语句 从入门到精通
开发语言·人工智能·python·opencv·支持向量机·计算机视觉
轻松Ai享生活2 小时前
GitHub Repo 骨架:Makefile + CUDA 入门程序
人工智能