本文主要是介绍把点云(point cloud)视作位姿的平移部分, 导出 Tum 格式的位姿,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!
需求:对一个 [N,3] 的 point cloud txt 补充 时间戳跟 单位阵 四元数。输出 Tum 格式的 位姿 txt。方便对 原始 point cloud 使用 EVO 轨迹评测工具,在 相似变换的 尺度上计算两个 点云的整体距离。
import os
import numpy as npdef 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()# 添加时间戳,零值和1output_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)
这篇关于把点云(point cloud)视作位姿的平移部分, 导出 Tum 格式的位姿的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!