对相机位姿 导出 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)
相关推荐
JAVA学习通8 分钟前
PostgreSQL 的 hstore、arrays 数据类型
人工智能·自然语言处理
B站计算机毕业设计之家1 小时前
多模态项目:Python人脸表情系统 CNN算法 神经网络+Adaboost定位+PyQt5界面 源码+文档 深度学习实战✅
python·深度学习·神经网络·opencv·yolo·计算机视觉·情绪识别
AKAMAI1 小时前
云成本困境:开支激增正阻碍欧洲AI创新
人工智能·云原生·云计算
大模型真好玩1 小时前
LangGraph实战项目:从零手搓DeepResearch(一)——DeepResearch应用体系详细介绍
人工智能·python·mcp
IT古董1 小时前
【第五章:计算机视觉-项目实战之生成式算法实战:扩散模型】3.生成式算法实战:扩散模型-(4)在新数据集上微调现有扩散模型
人工智能
嵌入式-老费1 小时前
Easyx图形库使用(潜力无限的图像处理)
图像处理·人工智能
JXY_AI1 小时前
AI问答与搜索引擎:信息获取的现状
人工智能·搜索引擎
B站_计算机毕业设计之家2 小时前
Python+Flask+Prophet 汽车之家二手车系统 逻辑回归 二手车推荐系统 机器学习(逻辑回归+Echarts 源码+文档)✅
大数据·人工智能·python·机器学习·数据分析·汽车·大屏端
XXX-X-XXJ2 小时前
三、从 MinIO 存储到 OCR 提取,再到向量索引生成
人工智能·后端·python·ocr
AI人工智能+2 小时前
行驶证识别技术通过OCR和AI实现信息自动化采集与处理,涵盖图像预处理、文字识别及结构化校验,提升效率与准确性
人工智能·深度学习·ocr·行驶证识别