对相机位姿 导出 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)
相关推荐
小陈phd10 分钟前
高级RAG策略学习(五)——llama_index实现上下文窗口增强检索RAG
人工智能
凯禾瑞华养老实训室2 小时前
人才教育导向下:老年生活照护实训室助力提升学生老年照护服务能力
人工智能
湫兮之风3 小时前
Opencv: cv::LUT()深入解析图像块快速查表变换
人工智能·opencv·计算机视觉
Christo33 小时前
TFS-2018《On the convergence of the sparse possibilistic c-means algorithm》
人工智能·算法·机器学习·数据挖掘
qq_508823403 小时前
金融量化指标--2Alpha 阿尔法
大数据·人工智能
黑金IT4 小时前
`.cursorrules` 与 `.cursorcontext`:Cursor AI 编程助手时代下的“双轨配置”指南
人工智能
学弟4 小时前
快捷:常见ocr学术数据集预处理版本汇总(适配mmocr)
计算机视觉
dlraba8024 小时前
基于 OpenCV 的信用卡数字识别:从原理到实现
人工智能·opencv·计算机视觉
IMER SIMPLE5 小时前
人工智能-python-深度学习-经典神经网络AlexNet
人工智能·python·深度学习
小憩-6 小时前
【机器学习】吴恩达机器学习笔记
人工智能·笔记·机器学习