3、轨迹表示与文件格式：TUM、KITTI、EuRoC数据集轨迹格式解析，时间戳对齐的重要性
做SLAM轨迹评估，第一步就是搞清楚轨迹长什么样。说白了，轨迹就是一系列带时间戳的位姿数据。但不同数据集，格式完全不同。我刚开始接触时，就被这些格式搞晕过。今天咱们一次性把它们理清楚。
3.1 轨迹的本质：时间戳 + 位姿
一条轨迹，本质上是一个序列。每个元素包含两个信息：
- 时间戳：记录这个位姿是什么时候采集的
- 位姿：通常是6自由度（位置x,y,z + 姿态四元数或旋转矩阵）
你想想看，如果时间戳对不上，那评估就毫无意义。我在项目中遇到过，有人直接用文件行号对齐，结果轨迹评估误差大得离谱——后来发现是时间戳没对齐。
核心原则：轨迹评估的前提是时间戳严格对齐。差之毫厘，谬以千里。
3.2 TUM格式：最直观的文本格式
TUM数据集（慕尼黑工业大学）的轨迹格式，我个人觉得是最容易理解的。每行一个位姿，格式如下：
# timestamp tx ty tz qx qy qz qw
1305031453.359375 1.234 2.345 3.456 0.123 0.234 0.345 0.789
1305031453.375000 1.235 2.346 3.457 0.124 0.235 0.346 0.788
...字段说明：
| 字段 | 含义 | 单位 |
|---|---|---|
| timestamp | Unix时间戳（秒） | 秒，浮点数 |
| tx, ty, tz | 相机位置（世界坐标系） | 米 |
| qx, qy, qz, qw | 姿态四元数（w在最后） | 无量纲 |
小技巧：TUM格式的四元数顺序是 qx, qy, qz, qw。很多库默认是 qw, qx, qy, qz。读取时一定要检查，我吃过这个亏。
3.3 KITTI格式：简洁但容易混淆
KITTI数据集（卡尔斯鲁厄理工学院）的轨迹格式，嗯，这里要注意——它没有时间戳！每行是一个3x4的变换矩阵，按行排列：
# 每行12个数字，对应3x4矩阵
1.0 0.0 0.0 0.0 0.0 1.0 0.0 0.0 0.0 0.0 1.0 0.0
0.999 0.012 -0.015 0.1 -0.012 0.999 0.008 0.2 0.015 -0.008 0.999 0.3
...这个矩阵是：
[R | t] 其中R是3x3旋转矩阵，t是3x1平移向量为什么没有时间戳？因为KITTI的轨迹文件通常对应图像序列的帧号。第i行就是第i帧的位姿。但这样做有个大坑——如果数据有丢帧，你的轨迹就全乱了。
警告：KITTI格式没有显式时间戳。如果你需要做时间对齐，必须从其他文件（如oxts数据）中获取时间信息。我曾经因为忽略这一点，评估结果差了30%。
3.4 EuRoC格式：带IMU的复杂格式
EuRoC数据集（苏黎世联邦理工学院）的轨迹格式稍微复杂一些。它包含两种数据：
1. 状态估计文件（state_groundtruth_estimate0/data.csv）：
# timestamp, p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_w [], q_RS_x [], q_RS_y [], q_RS_z [], v_RS_R_x [m s^-1], ...
1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ...
1.01, 0.01, 0.0, 0.0, 0.999, 0.005, 0.0, 0.0, 0.1, 0.0, 0.0, ...2. IMU数据（imu0/data.csv）：
# timestamp, omega_x, omega_y, omega_z, alpha_x, alpha_y, alpha_z
1.0, 0.01, 0.0, 0.0, 0.1, 0.0, 0.0
1.005, 0.011, 0.0, 0.0, 0.11, 0.0, 0.0注意：EuRoC的四元数顺序是 qw, qx, qy, qz，和TUM正好相反。而且时间戳单位是秒，但精度很高（纳秒级）。
3.5 时间戳对齐：评估的灵魂
为什么时间戳对齐这么重要？我给你举个例子。假设你的SLAM算法输出轨迹和真值轨迹如下：
| SLAM轨迹 | 真值轨迹 |
|---|---|
| 时间戳: 1.000, 位姿A | 时间戳: 1.010, 位姿A' |
| 时间戳: 2.000, 位姿B | 时间戳: 2.010, 位姿B' |
| 时间戳: 3.000, 位姿C | 时间戳: 3.010, 位姿C' |
如果直接按行对齐，你会把A和A'比较，B和B'比较。但实际时间差了10毫秒。对于快速运动的相机，10毫秒可能产生几厘米的位移误差。这个误差会直接算进你的ATE（绝对轨迹误差）里，让你误以为算法很差。
正确的做法：使用线性插值或最近邻插值，将SLAM轨迹和真值轨迹对齐到同一时间戳序列上。
我常用的对齐代码（Python）：
import numpy as np
from scipy.interpolate import interp1d
def align_timestamps(slam_ts, slam_poses, gt_ts, gt_poses):
"""
将真值轨迹插值到SLAM轨迹的时间戳上
"""
# 对真值轨迹的每个维度进行插值
aligned_gt = []
for i in range(gt_poses.shape[1]):
f = interp1d(gt_ts, gt_poses[:, i], kind='linear',
bounds_error=False, fill_value='extrapolate')
aligned_gt.append(f(slam_ts))
return np.column_stack(aligned_gt)避坑指南：我曾经直接用最近邻插值，结果在快速转弯处误差暴增。后来改用线性插值，效果好了很多。如果你的数据频率很高（如200Hz），最近邻也够用；但如果是10Hz以下，一定要用线性插值。
3.6 格式转换：实用工具
实际工作中，你经常需要在不同格式间转换。我整理了一个简单的对照表：
| 源格式 | 目标格式 | 注意事项 |
|---|---|---|
| TUM → KITTI | 去掉时间戳，四元数转旋转矩阵 | 注意四元数顺序 |
| KITTI → TUM | 添加虚拟时间戳，旋转矩阵转四元数 | 时间戳需要从其他来源获取 |
| EuRoC → TUM | 提取位姿部分，调整四元数顺序 | EuRoC的qw在第一位 |
嗯，这里再分享一个我常用的转换函数片段：
def tum_to_kitti(tum_file, kitti_file):
"""将TUM格式转换为KITTI格式"""
data = np.loadtxt(tum_file)
timestamps = data[:, 0]
positions = data[:, 1:4]
quaternions = data[:, 4:8] # qx, qy, qz, qw
with open(kitti_file, 'w') as f:
for i in range(len(timestamps)):
# 四元数转旋转矩阵
R = quaternion_to_rotation_matrix(quaternions[i])
t = positions[i].reshape(3, 1)
T = np.hstack([R, t])
# 按行写入12个数字
f.write(' '.join(map(str, T.flatten())) + '\n')3.7 本章小结
轨迹格式看似简单，但细节决定成败。我见过太多人在评估时因为格式问题翻车。记住三点：
- 时间戳是命根子——对齐不好，评估白做
- 四元数顺序要确认——TUM和EuRoC正好相反
- KITTI没有时间戳——需要额外处理
下一节我们会深入ATE和RPE的具体计算方法。但如果你连轨迹格式都没搞明白，那些指标算出来也是错的。先把基础打牢。