4. 轨迹数据格式与存储：常见轨迹格式（TUM、KITTI、EuRoC）、ROS Bag文件、自定义数据格式、HDF5存储
做轨迹评估，第一关就是跟数据格式打交道。说实话，我早年刚入行时，被各种格式搞得晕头转向。TUM、KITTI、EuRoC，每个数据集都有自己的脾气。你想想看，如果连数据都读不对，后面的评估全是白搭。
这一章，我就带你把这些格式彻底捋清楚。咱们不光要认识它们，还要知道怎么存、怎么读、怎么转。
4.1 常见轨迹格式：三巨头
目前学术界和工业界，最常用的就是这三种格式。我按使用频率排个序，咱们一个个看。
4.1.1 TUM 格式
TUM 格式来自慕尼黑工业大学。它简单到令人发指，但也正因为简单，成了最通用的交换格式。
格式长这样：
# timestamp tx ty tz qx qy qz qw
1.234567 0.1 0.2 0.3 0.0 0.0 0.0 1.0
2.345678 0.2 0.3 0.4 0.1 0.0 0.0 0.99
每行8列：时间戳（秒）、平移xyz、四元数xyzw。注意，时间戳是浮点数，单位是秒。
读TUM格式的代码，我通常这样写：
def read_tum(file_path):
timestamps = []
positions = []
quaternions = []
with open(file_path, 'r') as f:
for line in f:
if line.startswith('#'):
continue
parts = line.strip().split()
if len(parts) < 8:
continue
t = float(parts[0])
tx, ty, tz = map(float, parts[1:4])
qx, qy, qz, qw = map(float, parts[4:8])
timestamps.append(t)
positions.append([tx, ty, tz])
quaternions.append([qx, qy, qz, qw])
return timestamps, positions, quaternions
4.1.2 KITTI 格式
KITTI 格式就「硬核」多了。它不存四元数，直接存3x4的变换矩阵。每行12个数，对应矩阵的3行4列。
r11 r12 r13 tx r21 r22 r23 ty r31 r32 r33 tz
注意，KITTI 格式没有时间戳。它假设数据是按帧顺序排列的。帧率通常是10Hz。
正确的读法：
def read_kitti_pose(file_path):
poses = []
with open(file_path, 'r') as f:
for line in f:
parts = list(map(float, line.strip().split()))
if len(parts) != 12:
continue
# 注意：这里是行主序
matrix = np.array(parts).reshape(3, 4)
poses.append(matrix)
return poses
4.1.3 EuRoC 格式
EuRoC 格式跟TUM很像，但多了一个东西：它用四元数表示旋转，但顺序是 qw qx qy qz。别小看这个顺序，搞反了旋转方向就反了。
# 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 []
1.234567 0.1 0.2 0.3 0.999 0.0 0.0 0.0
EuRoC 的时间戳是纳秒级的整数。我一般先转成秒再处理。
| 格式 | 时间戳 | 位置 | 旋转 | 典型应用 |
|---|---|---|---|---|
| TUM | 浮点秒 | xyz | qx qy qz qw | SLAM评估 |
| KITTI | 无（按帧） | 3x4矩阵 | 3x4矩阵 | 自动驾驶 |
| EuRoC | 纳秒整数 | xyz | qw qx qy qz | VIO评估 |
4.2 ROS Bag 文件
做机器人相关的工作，ROS Bag 是绕不开的。它本质上是一个容器，可以同时存多个topic的数据。轨迹数据通常放在 /odom、/tf 或 /gps 这些topic里。
我个人习惯用 rosbag 命令行工具先看看包里有什么：
rosbag info trajectory.bag
输出会告诉你有哪些topic、消息类型、消息数量。然后你可以用Python脚本提取：
import rosbag
from geometry_msgs.msg import PoseStamped
bag = rosbag.Bag('trajectory.bag')
timestamps = []
positions = []
for topic, msg, t in bag.read_messages(topics=['/odom']):
if isinstance(msg, PoseStamped):
timestamps.append(t.to_sec())
positions.append([
msg.pose.position.x,
msg.pose.position.y,
msg.pose.position.z
])
bag.close()
4.3 自定义数据格式
很多时候，现成的格式满足不了需求。比如你要存轨迹+IMU+图像时间戳，或者要存协方差矩阵。这时候就得自己定义格式。
我推荐用JSON或YAML做自定义格式。可读性好，扩展方便。
{
"format_version": "1.0",
"sensor": "lidar",
"trajectory": [
{
"timestamp": 1.234567,
"position": [0.1, 0.2, 0.3],
"orientation": [0.0, 0.0, 0.0, 1.0],
"covariance": [
[0.01, 0, 0],
[0, 0.01, 0],
[0, 0, 0.01]
]
}
]
}
读起来也简单：
import json
with open('trajectory.json', 'r') as f:
data = json.load(f)
poses = data['trajectory']
4.4 HDF5 存储
HDF5 是我最推荐的存储方案，尤其是数据量大的时候。它支持分层结构、压缩、并行读写。一个文件里可以存轨迹、图像、点云，互不干扰。
写HDF5的代码：
import h5py
import numpy as np
with h5py.File('trajectory.h5', 'w') as f:
# 创建数据集
f.create_dataset('timestamps', data=np.array(timestamps))
f.create_dataset('positions', data=np.array(positions))
f.create_dataset('quaternions', data=np.array(quaternions))
# 添加属性
f.attrs['sensor'] = 'camera'
f.attrs['date'] = '2024-01-01'
# 分组存储
grp = f.create_group('evaluation')
grp.create_dataset('ate', data=ate_value)
grp.create_dataset('rpe', data=rpe_value)
读HDF5：
with h5py.File('trajectory.h5', 'r') as f:
timestamps = f['timestamps'][:]
positions = f['positions'][:]
sensor = f.attrs['sensor']
- 支持部分读取：只读需要的帧，不用全加载
- 自带压缩：无损压缩能省50%-70%空间
- 跨平台：Windows、Linux、macOS通用
- 支持并行：多个进程同时读写不同区域
4.5 格式转换实战
实际项目中，你经常需要在不同格式间转换。我画了一张流程图，帮你理清思路：
转换的核心思路就一条：先统一成中间格式，再转目标格式。这样你只需要写N个转换器，而不是N×N个。
好了，这一章的内容就到这里。数据格式是轨迹评估的地基，地基打不牢，后面全是空中楼阁。下一章咱们会深入轨迹对齐和评估指标，到时候这些格式知识都会用上。