4、轨迹数据格式：TUM格式、KITTI格式、EuRoC格式、ROS bag格式
做SLAM定位评估，说白了就是跟轨迹数据打交道。我见过不少新手，算法写得挺溜，结果在数据格式转换上栽了跟头。嗯，今天咱们就把这几种主流格式掰扯清楚。
核心观点：轨迹数据格式是评估的「通用语言」。格式不对，评估白费。
4.1 TUM格式——最直观的「时间戳+位姿」
TUM格式来自慕尼黑工业大学，是学术界用得最多的格式之一。我个人习惯用它做快速验证，因为结构简单到一眼就能看懂。
每行数据包含：时间戳 tx ty tz qx qy qz qw
举个例子：
# 时间戳(秒) tx(m) ty(m) tz(m) qx qy qz qw
1403715273.542 1.234 0.567 0.123 0.707 0.000 0.707 0.000
1403715273.643 1.245 0.578 0.131 0.708 0.001 0.706 0.001这里要注意几个坑：
- 时间戳单位是秒，不是纳秒。我在项目中遇到过有人把ROS的纳秒时间戳直接塞进来，结果轨迹全乱了。
- 四元数必须归一化。我曾经调试一个诡异问题，折腾了两天，最后发现是四元数模长不是1。
- 平移单位是米，别跟毫米搞混了。
我的小技巧：写一个Python脚本，用numpy.loadtxt()直接读TUM格式，然后转成Nx7的矩阵，处理起来特别顺手。
4.2 KITTI格式——自动驾驶界的「老大哥」
KITTI格式跟TUM最大的区别是：它用3x4的变换矩阵，而不是分开的平移+四元数。你想想看，自动驾驶领域对实时性要求高，矩阵运算比四元数转换要快不少。
格式长这样：
# 每行12个数字，对应3x4变换矩阵（行优先）
r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz实际数据：
0.9999 -0.0100 0.0100 1.234
0.0100 0.9999 -0.0100 0.567
-0.0100 0.0100 0.9999 0.123嗯，这里有个容易踩的坑：KITTI格式没有时间戳。它默认每帧按顺序排列，帧率固定10Hz。如果你用不同帧率的数据去评估，记得先做时间对齐。
警告：KITTI的坐标系是「右-下-前」（RGB相机坐标系），而TUM是「左-上-前」（惯用坐标系）。直接拿来对比会出大问题。我刚开始做评估时就吃过这个亏。
4.3 EuRoC格式——微小型飞行器的「标准答案」
EuRoC数据集来自苏黎世联邦理工学院，主要针对无人机场景。它的格式跟TUM很像，但多了一个关键字段：协方差矩阵。
格式：时间戳 tx ty tz qx qy qz qw 协方差(6x6上三角)
为什么要加协方差？说白了，无人机飞得快、抖动大，每个位姿的置信度不一样。有了协方差，评估工具就能知道哪些点「靠谱」，哪些点「可疑」。
实际数据片段：
1403715273.542 1.234 0.567 0.123 0.707 0.000 0.707 0.000
0.001 0.000 0.000 0.000 0.000 0.000
0.001 0.000 0.000 0.000 0.000
0.001 0.000 0.000 0.000
0.001 0.000 0.000
0.001 0.000
0.001注意看，协方差矩阵只存上三角部分，共21个数字。我建议你写解析代码时，先读前8个字段（时间戳+位姿），再读21个协方差值，别搞混了。
4.4 ROS bag格式——机器人界的「万能容器」
ROS bag不是专门的轨迹格式，它是一个容器，可以装任意ROS消息。轨迹数据通常以nav_msgs/Odometry或geometry_msgs/PoseStamped的形式存在bag里。
我个人觉得，bag格式最大的好处是：时间戳自动对齐。ROS的rosbag play会按照录制时的真实时间戳回放数据，省去了手动同步的麻烦。
提取轨迹的常用命令：
# 从bag中提取odometry话题
rostopic echo -b your_bag.bag -p /odometry > trajectory.txt
# 或者用Python脚本
import rosbag
bag = rosbag.Bag('your_bag.bag')
for topic, msg, t in bag.read_messages(topics=['/odometry']):
# 提取位置和姿态
pos = msg.pose.pose.position
ori = msg.pose.pose.orientation
print(f"{t.to_sec()} {pos.x} {pos.y} {pos.z} {ori.x} {ori.y} {ori.z} {ori.w}")避坑指南：我曾经用rostopic echo -p导出数据，发现时间戳精度丢失了。后来改用Python脚本直接读msg.header.stamp，精度才保住了。记住，-p选项会截断纳秒部分。
4.5 四种格式对比一览
| 特性 | TUM | KITTI | EuRoC | ROS bag |
|---|---|---|---|---|
| 位姿表示 | 平移+四元数 | 3x4矩阵 | 平移+四元数+协方差 | ROS消息类型 |
| 时间戳 | 有（秒） | 无（按帧序） | 有（秒） | 有（纳秒） |
| 适用场景 | 学术评估 | 自动驾驶 | 无人机/VIO | 机器人系统 |
| 文件大小 | 小 | 中 | 大（含协方差） | 很大（含传感器数据） |
| 解析难度 | 简单 | 中等 | 中等 | 需要ROS环境 |
4.6 格式转换实战
实际工作中，你经常需要在不同格式之间来回切换。我整理了一个通用的转换思路：
- 统一中间格式：先把所有格式转成TUM格式（因为最简单）
- 时间戳对齐：用线性插值或最近邻插值对齐时间戳
- 坐标系转换：注意不同数据集的坐标系定义
- 输出目标格式：从中间格式转成你需要的格式
举个例子，KITTI转TUM的核心代码：
import numpy as np
def kitti_to_tum(kitti_pose):
"""
kitti_pose: 3x4 变换矩阵
返回: [tx, ty, tz, qx, qy, qz, qw]
"""
# 提取平移
tx, ty, tz = kitti_pose[0,3], kitti_pose[1,3], kitti_pose[2,3]
# 提取旋转矩阵并转四元数
R = kitti_pose[:3,:3]
qw = np.sqrt(1 + R[0,0] + R[1,1] + R[2,2]) / 2
qx = (R[2,1] - R[1,2]) / (4 * qw)
qy = (R[0,2] - R[2,0]) / (4 * qw)
qz = (R[1,0] - R[0,1]) / (4 * qw)
return [tx, ty, tz, qx, qy, qz, qw]重要提醒：四元数转换时，如果qw接近0（旋转180度的情况），要用其他公式。我建议用scipy的Rotation.from_matrix()，它内部处理了所有边界情况。
4.7 知识体系总览
下面这张图总结了四种格式的核心关系，以及它们在实际评估流程中的位置：
从这张图可以看得很清楚：不管你用什么格式采集数据，最终都要经过「统一中间格式」这个环节，才能进行公平的轨迹评估。我建议你把这个流程固化到自己的工具链里，每次做评估都走一遍，能省下不少调试时间。