把点云（point cloud）视作位姿的平移部分，导出 Tum 格式的位姿

培之

已于 2023-12-20 14:44:21 修改

阅读量552

点赞数 8

CC 4.0 BY-SA版权

分类专栏：代码库文章标签： python 计算机视觉

于 2023-12-20 14:32:32 首次发布

本文链接：https://blog.youkuaiyun.com/OrdinaryMatthew/article/details/135107385

代码库专栏收录该内容

8 篇文章

订阅专栏

本文介绍了一个Python函数，用于给给定的N×3pointcloudtxt文件添加时间戳和单位阵四元数，将其转换为Tum格式的位姿txt，以便于使用EVO轨迹评测工具计算点云间的相似变换距离。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

需求：对一个 [N,3] 的 point cloud txt 补充时间戳跟单位阵四元数。输出 Tum 格式的位姿 txt。方便对原始 point cloud 使用 EVO 轨迹评测工具，在相似变换的尺度上计算两个点云的整体距离。

import os
import numpy as np

def transform_and_write_txt(input_file, output_file):
    # 读取输入文件
    with open(input_file, "r") as f:
        lines = f.readlines()

    # 生成时间戳
    timestamps = np.linspace(0, 1000, len(lines), endpoint=False)

    # 处理每一行
    output_lines = []
    for timestamp, line in zip(timestamps, lines):
        # 分割每一行的坐标值
        coords = line.strip().split()

        # 添加时间戳，零值和1
        output_line = [f"{timestamp:.6f}"] + coords + ["0", "0", "0", "1"]

        # 将该行数据转为字符串并添加到输出列表
        output_lines.append(" ".join(output_line))

    # 将处理后的数据写入输出文件
    with open(output_file, "w") as f:
        f.write("\n".join(output_lines))

    print(f"Output written to {output_file}")

current_directory = os.path.dirname(os.path.abspath(__file__))
input_file1 =   os.path.join(current_directory , "LoFTR_pt.txt") 
output_file1 =  os.path.join(current_directory , 'Tum'+os.path.basename(input_file1)) 

transform_and_write_txt(input_file1, output_file1)