LIO-SAM轨迹数据格式转换：从PCD到TUM的实践指南

1. 为什么我们需要转换LIO-SAM的轨迹格式？

如果你用过LIO-SAM，肯定对它的建图能力印象深刻。它能把激光雷达和IMU的数据揉在一起，实时构建出高精度的三维点云地图，同时还能输出机器人的运动轨迹。但问题来了，当你兴冲冲地想把这条轨迹拿出来，跟其他算法（比如ORB-SLAM3、VINS-Mono）做个对比，或者用evo这类神器评估一下精度时，往往会卡在第一步：格式不兼容。

LIO-SAM默认把轨迹点存成了PCD（Point Cloud Data）格式。这玩意儿存点云是极好的，一个文件里能塞下几十万个点的三维坐标。但对于轨迹来说，PCD文件里存的其实是一系列带有时间戳和六自由度位姿（位置+姿态）的“关键帧”。直接拿这个PCD文件去喂给evo，evo会一脸懵，因为它只认识TUM、KITTI、Euroc这些标准格式。这就好比你想用中文跟一个只懂英文的朋友聊天，必须得有个翻译过程。

我自己在项目里就遇到过这个坎。当时需要把LIO-SAM的结果和一套视觉惯性里程计的结果做融合，第一步就是得把轨迹对齐。两个系统输出的格式完全不同，直接比较就是鸡同鸭讲。折腾了半天，最后发现最靠谱的办法，就是从LIO-SAM的源码入手，在它保存地图的那个关键时刻，把轨迹信息按照TUM格式的规则，“顺手”给写出来。这个过程听起来有点硬核，但实际操作起来，就像给一个现成的工具加个顺手的小功能，一旦打通，后面所有评估、对比、融合的流程就都顺了。

所以，这篇文章就是来填这个坑的。我会手把手带你走一遍完整的流程：从理解LIO-SAM内部怎么存轨迹数据，到剖析TUM格式的每一个字段要求，最后给出可以直接复制粘贴的代码修改方案。目标很简单：让你在跑完LIO-SAM后，能多拿到一个标准的TUM格式轨迹文件，为后续的所有分析工作铺平道路。

2. 理解两种格式：PCD与TUM的本质区别

要搞定转换，首先得明白你在转什么。LIO-SAM输出的PCD轨迹，和TUM格式的轨迹，虽然描述的是同一段机器人运动，但它们的“语言”完全不同。

LIO-SAM的PCD轨迹文件里有什么？

LIO-SAM内部用两个点云来管理轨迹：cloudKeyPoses3D 和 cloudKeyPoses6D。顾名思义，3D那个只存位置（x, y, z），6D那个存的是完整的六自由度位姿（x, y, z, roll, pitch, yaw）。注意，这里的roll、pitch、yaw是欧拉角，单位是弧度。它用PCD格式保存，本质上是一个特殊的点云，每个“点”不再代表空间中的一个位置，而是代表一个关键帧时刻的机器人状态。除了位姿，这些点里还可能包含时间戳（time字段）等自定义数据。你直接用文本编辑器打开trajectory.pcd可能会看到一堆二进制数据，不方便直接阅读和解析。

TUM格式的规则是什么？

TUM格式是慕尼黑工业大学（TUM）为他们的RGB-D数据集设计的一种轨迹文件格式，因为简单明了，后来成了机器人领域评估轨迹的“普通话”。它是一个纯文本文件，每一行代表一个位姿。每一行有8个数字，用空格分隔：

timestamp tx ty tz qx qy qz qw

我来拆解一下：

• timestamp： 时间戳，单位是秒。这是一个浮点数。关键点在于，它通常要求是相对时间，从轨迹开始算起，而不是绝对的UNIX时间戳。
• tx ty tz： 平移向量，也就是机器人在世界坐标系下的位置（x, y, z），单位是米。
• qx qy qz qw： 表示旋转的四元数。这是和LIO-SAM最大的不同！TUM格式用四元数来描述姿态，而不是欧拉角。

这里有个至关重要的细节：四元数的顺序。TUM格式采用的是 (qx, qy, qz, qw)，其中qw是标量部分（实部）。这个顺序一定要记牢，很多错误都出在这里。而LIO-SAM内部用的是欧拉角（roll绕X轴，pitch绕Y轴，yaw绕Z轴），所以转换的核心步骤之一，就是把欧拉角转换成这个特定顺序的四元数。

简单总结一下，格式转换就像一次“数据翻译”：

1. 数据源：从LIO-SAM的 cloudKeyPoses6D 中，取出每个关键帧的时间戳、位置（x,y,z）和姿态角（roll, pitch, yaw）。
2. 计算：将欧拉角（弧度制）转换为四元数（顺序为x, y, z, w）。
3. 格式化：将时间戳（处理为相对时间）、平移坐标和四元数，按照“8个数字空格分隔”的规则，写入一个文本文件。
4. 调整（可选）：根据你的坐标系定义，可能需要对位置坐标（x, y, z）进行正负号的调整。比如原始代码中的 -1*