3. 轨迹表示方法：时间戳序列、位姿列表、轨迹文件格式
做SLAM评估这么多年，我见过最多的坑，就是轨迹表示不一致导致的评估结果偏差。说白了，你拿TUM格式的轨迹去跑KITTI的评估脚本，结果肯定是一团糟。今天我们就来彻底搞清楚，轨迹到底该怎么表示，不同格式之间有什么区别。
3.1 时间戳序列：轨迹的“时间轴”
轨迹本质上是一个随时间变化的位姿序列。每个位姿都对应一个时间戳，这个时间戳就是轨迹的“时间轴”。
时间戳的精度，我个人习惯用微秒（us）或纳秒（ns）级别。为什么？因为SLAM系统通常运行在10-30Hz，帧间间隔也就33-100ms。如果时间戳精度只有毫秒，多个帧可能共享同一个时间戳，评估时就乱套了。
关键点：时间戳必须单调递增，不能回退。我在项目中遇到过，某个传感器驱动的时间戳偶尔会跳变回几秒前，结果轨迹评估直接报错。
时间戳的格式，常见的有两种：
- Unix时间戳（浮点数）：例如 1234567890.123456，单位是秒。TUM格式就用这个。
- 系统时间（整数）：例如 1234567890123456，单位是微秒或纳秒。EuRoC格式用这个。
你想想看，如果两个轨迹的时间戳基准不一致，比如一个用UTC，一个用系统启动时间，那对齐的时候就会出大问题。所以，拿到轨迹数据的第一件事，就是检查时间戳的基准和单位。
3.2 位姿列表：轨迹的“空间骨架”
每个时间戳对应一个位姿。位姿通常包含位置和姿态两部分。
位置：一般用三维坐标 (x, y, z) 表示。单位通常是米。
姿态：表示方式就多了，常见的有：
- 四元数 (qx, qy, qz, qw)：最常用，无奇异性。TUM和EuRoC都用这个。
- 旋转矩阵 (3x3)：直观，但占空间。KITTI格式用这个。
- 欧拉角 (roll, pitch, yaw)：容易理解，但有万向锁问题。我一般不建议在轨迹评估中用。
我的经验：四元数虽然不直观，但做插值和融合时最稳定。我曾经因为用欧拉角做轨迹插值，结果在俯仰角接近90度时，评估结果完全失真。从那以后，我所有轨迹处理都统一用四元数。
位姿列表的存储，说白了就是一个表格。每一行是一个时间戳+一个位姿。但不同数据集格式，列的排列顺序不一样。这就是我们接下来要讲的。
3.3 三大主流轨迹文件格式
做SLAM评估，你一定会遇到这三种格式。我分别说一下它们的结构和注意事项。
3.3.1 TUM格式
TUM格式是慕尼黑工业大学提出的，非常简洁。每行包含8个字段：
timestamp tx ty tz qx qy qz qw时间戳是浮点数，单位秒。位置和四元数都是双精度浮点数。
特点：
- 没有表头，直接是数据行。
- 时间戳精度高，通常到微秒级。
- 姿态用四元数，无歧义。
注意：我曾经遇到过，有人把TUM格式的四元数顺序写成了 (qw, qx, qy, qz)，结果评估出来的轨迹完全不对。一定要确认顺序是 (qx, qy, qz, qw)。
3.3.2 KITTI格式
KITTI格式来自KITTI数据集，它用旋转矩阵表示姿态。每行包含13个字段：
r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz注意，KITTI格式没有时间戳！它假设所有位姿是按时间顺序排列的，帧率固定。
特点：
- 没有时间戳，评估时只能按顺序对齐。
- 旋转矩阵是3x3，共9个元素，加上位置3个，共12个。但KITTI格式通常写成一行12个数字，或者写成4x4齐次矩阵的12个元素（去掉最后一行）。
- 单位是米。
避坑指南：KITTI格式的轨迹，通常是从第一帧开始的相对位姿。如果你要评估绝对轨迹误差（ATE），需要先把轨迹转换到全局坐标系下。我刚开始做KITTI评估时，就因为这个吃了亏。
3.3.3 EuRoC格式
EuRoC格式来自EuRoC MAV数据集，它用四元数表示姿态，但时间戳是整数纳秒。每行包含8个字段：
timestamp_ns tx ty tz qx qy qz qw特点：
- 时间戳是整数，单位纳秒。范围很大，处理时要注意数据类型，别用32位整数，会溢出。
- 姿态用四元数，和TUM一样。
- 通常还包含IMU数据，但轨迹文件只包含位姿。
嗯，这里要注意，EuRoC格式的时间戳是相对于数据集开始的，不是Unix时间戳。所以不同数据集的时间戳不能直接比较。
3.4 格式对比与转换
为了方便你理解，我做了一个对比表：
| 格式 | 时间戳 | 姿态表示 | 字段数 | 典型应用 |
|---|---|---|---|---|
| TUM | 浮点秒 | 四元数 (qx, qy, qz, qw) | 8 | RGB-D SLAM评估 |
| KITTI | 无（按顺序） | 旋转矩阵 (3x3) | 12 | 自动驾驶SLAM评估 |
| EuRoC | 整数纳秒 | 四元数 (qx, qy, qz, qw) | 8 | 视觉-惯性SLAM评估 |
格式转换是家常便饭。我常用的转换逻辑：
- TUM ↔ EuRoC：时间戳单位转换（秒↔纳秒），字段顺序不变。
- TUM ↔ KITTI：四元数转旋转矩阵（或反过来），时间戳处理（KITTI需要生成虚拟时间戳）。
- KITTI ↔ EuRoC：旋转矩阵转四元数，时间戳处理。
我的建议：统一用TUM格式作为中间格式。因为TUM格式最简单，而且大多数评估工具（如evo）原生支持TUM。我自己的工具链，都是先把所有轨迹转成TUM，再做后续处理。
3.5 知识体系结构图
下面这张图，帮你理清轨迹表示的核心逻辑：
说白了，轨迹表示就是“时间戳+位姿”的组合。不同格式只是这个组合的不同序列化方式。理解了这一点，你就能轻松应对各种格式的转换和评估。
总结一下：
- 时间戳是轨迹的灵魂，精度和基准必须统一。
- 位姿用四元数最稳妥，避免欧拉角的奇异性问题。
- TUM格式最通用，建议作为中间格式。
- KITTI格式没有时间戳，评估时要注意对齐方式。
- EuRoC格式时间戳是纳秒整数，处理时注意数据类型。
好了，轨迹表示这部分就讲到这里。记住，格式转换时多检查一遍字段顺序和单位，能省去后面很多调试时间。我当年就是因为没注意四元数顺序，多花了一整天排查问题。