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

需求：对一个 [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)