对相机位姿 导出 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)
相关推荐
禁默2 小时前
第六届大数据、人工智能与物联网工程国际会议(ICBAIE 2025)
大数据·人工智能·物联网
chxin140162 小时前
openCV3.0 C++ 学习笔记补充(自用 代码+注释)---持续更新 四(91-)
c++·opencv·计算机视觉
茜茜西西CeCe2 小时前
数字图像处理-巴特沃斯高通滤波、低通滤波
图像处理·opencv·计算机视觉·matlab·巴特沃斯高通滤波·巴特沃斯低通滤波
天天爱吃肉82182 小时前
【比亚迪璇玑架构深度解析:重新定义智能电动汽车的“整车智能”】
数据库·人工智能·嵌入式硬件·架构·汽车
semantist@语校3 小时前
第十九篇|东京世界日本语学校的结构数据建模:制度函数、能力矩阵与升学图谱
数据库·人工智能·线性代数·矩阵·prompt·github·数据集
guygg883 小时前
HOG + SVM 行人检测
人工智能·机器学习·支持向量机
和鲸社区3 小时前
四大经典案例,入门AI算法应用,含分类、回归与特征工程|2025人工智能实训季初阶赛
人工智能·python·深度学习·算法·机器学习·分类·回归
IT古董3 小时前
【第五章:计算机视觉】1.计算机视觉基础-(3)卷积神经网络核心层与架构分析:卷积层、池化层、归一化层、激活层
人工智能·计算机视觉·cnn
黎燃4 小时前
AI生成音乐的创作逻辑深析:以AIVA为例
人工智能
点云SLAM4 小时前
四元数 (Quaternion)在位姿(SE(3))表示下的各类导数(雅可比)知识(2)
人工智能·线性代数·算法·机器学习·slam·四元数·李群李代数