对相机位姿 导出 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)
相关推荐
学术头条15 分钟前
AI 的「phone use」竟是这样练成的,清华、智谱团队发布 AutoGLM 技术报告
人工智能·科技·深度学习·语言模型
准橙考典17 分钟前
怎么能更好的通过驾考呢?
人工智能·笔记·自动驾驶·汽车·学习方法
ai_xiaogui20 分钟前
AIStarter教程:快速学会卸载AI项目【AI项目管理平台】
人工智能·ai作画·语音识别·ai写作·ai软件
孙同学要努力24 分钟前
《深度学习》——深度学习基础知识(全连接神经网络)
人工智能·深度学习·神经网络
喵~来学编程啦1 小时前
【论文精读】LPT: Long-tailed prompt tuning for image classification
人工智能·深度学习·机器学习·计算机视觉·论文笔记
深圳市青牛科技实业有限公司1 小时前
【青牛科技】应用方案|D2587A高压大电流DC-DC
人工智能·科技·单片机·嵌入式硬件·机器人·安防监控
水豚AI课代表2 小时前
分析报告、调研报告、工作方案等的提示词
大数据·人工智能·学习·chatgpt·aigc
几两春秋梦_2 小时前
符号回归概念
人工智能·数据挖掘·回归
用户691581141653 小时前
Ascend Extension for PyTorch的源码解析
人工智能
用户691581141653 小时前
Ascend C的编程模型
人工智能