一、轨迹回放基础：从数据到可视化

做SLAM的人，每天打交道最多的东西是什么？我猜就是轨迹了。

你想想看，一个SLAM系统跑完，你拿到手的无非就是两样东西：一张地图，一条轨迹。地图咱们后面再聊，今天先说说轨迹。说白了，轨迹就是相机或机器人在空间里走过的路径，是一连串带时间戳的位姿数据。

我个人习惯把轨迹数据看作「时空坐标的集合」。每个点都包含三个信息：什么时间、在什么位置、朝向哪里。就这么简单，但里面的门道可不少。

1.1 三种最常见的轨迹格式

我在项目里接触过的轨迹格式少说也有十几种，但真正称得上「行业标准」的，就下面这三个。你只要搞懂了它们，其他格式基本都能触类旁通。

TUM 格式

TUM 格式来自慕尼黑工业大学，是学术界用得最广的格式之一。它的结构非常直观：

# timestamp tx ty tz qx qy qz qw
1403636579.647200 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
1403636579.648200 0.001000 -0.001000 0.000000 0.000000 0.000000 0.000000 1.000000
...

每行8列，分别是时间戳、位置xyz、四元数xyzw。嗯，这里要注意：四元数的顺序是 qx qy qz qw，不是 qw qx qy qz。我曾经在这个顺序上吃过亏，跑完评估发现轨迹误差大得离谱，查了半天才发现是四元数顺序搞反了。

KITTI 格式

KITTI 格式就有点不一样了。它不直接给你位置和朝向，而是给一个 3x4 的变换矩阵：

# 每行12个数字，构成 3x4 变换矩阵 [R | t]
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.9999 -0.0098 0.0072 -0.0001 0.0100 0.9999 -0.0043 -0.0001 -0.0072 0.0044 1.0000 0.0001
...

为什么 KITTI 要用矩阵而不是分开的位置+四元数？我个人理解，这跟 KITTI 数据集主要面向自动驾驶有关。自动驾驶里经常需要做坐标系变换，矩阵形式用起来更直接。你想想看，如果你要算两个位姿之间的相对变换，矩阵乘法一步就搞定了。

EuRoC 格式

EuRoC 格式跟 TUM 很像，但多了一个东西——时间戳的单位是纳秒。而且它的数据通常包含 IMU 信息和视觉信息，轨迹只是其中一部分。

# timestamp [ns], p_RS_R_x [m], p_RS_R_y [m], p_RS_R_z [m], q_RS_R_w [], q_RS_R_x [], q_RS_R_y [], q_RS_R_z []
1403636579647200000 0.000000 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000
...

看到那个 1403636579647200000 了吗？这是纳秒级的时间戳。如果你要跟 TUM 格式混用，记得除以 1e9 转换成秒。我刚开始用 EuRoC 时，直接把时间戳当秒用了，结果轨迹对齐全乱套了。

1.2 三种格式对比

特性	TUM	KITTI	EuRoC
时间戳单位	秒 (浮点)	无 (按帧索引)	纳秒 (整数)
位姿表示	位置 + 四元数	3x4 变换矩阵	位置 + 四元数
适用场景	室内SLAM评估	自动驾驶	无人机/VIO
文件扩展名	.txt 或 .tum	.txt	.csv 或 .txt

1.3 轨迹可视化：evo 和 plotly

数据拿到手了，怎么看？我推荐两个工具：evo 和 plotly。前者是SLAM圈的老牌工具，后者是通用可视化库，但用在轨迹上效果也很好。

evo：SLAMer 的瑞士军刀

evo 是我用得最多的轨迹工具，没有之一。它最牛的地方是：一行命令就能画出轨迹对比图。

# 画单条轨迹
evo_traj tum trajectory.txt --plot

# 对比两条轨迹
evo_traj tum estimated.txt ground_truth.txt --plot

# 计算绝对轨迹误差 (ATE)
evo_ape tum ground_truth.txt estimated.txt --plot

# 计算相对位姿误差 (RPE)
evo_rpe tum ground_truth.txt estimated.txt --plot

我记得有一次做项目，客户说我们的SLAM精度不行。我直接用 evo 跑了个 ATE 分析，把误差分布图甩过去，对方立马闭嘴了。为什么？因为可视化比数字更有说服力。

plotly：交互式轨迹可视化

evo 适合快速分析，但如果你想做更炫酷的交互式可视化，plotly 是更好的选择。它支持鼠标拖拽旋转、缩放、悬停显示数据点信息。

import plotly.graph_objects as go
import numpy as np

# 假设你已经加载了轨迹数据
trajectory = np.loadtxt('trajectory.txt')
x, y, z = trajectory[:, 1], trajectory[:, 2], trajectory[:, 3]

fig = go.Figure(data=[
    go.Scatter3d(
        x=x, y=y, z=z,
        mode='lines+markers',
        marker=dict(size=2, color=np.arange(len(x)), colorscale='Viridis'),
        line=dict(color='darkblue', width=2)
    )
])

fig.update_layout(
    scene=dict(
        xaxis_title='X (m)',
        yaxis_title='Y (m)',
        zaxis_title='Z (m)'
    ),
    title='3D Trajectory Visualization'
)

fig.show()

这段代码会生成一个3D轨迹图，颜色从紫色渐变到黄色，表示时间顺序。你想想看，用鼠标拖拽旋转，从各个角度观察轨迹，是不是比看一堆数字直观多了？

1.4 本章知识体系

下面这张图总结了本章的核心内容，你可以把它当作一个快速参考：

1.5 写在最后

轨迹数据看着简单，但它是SLAM系统最直接的输出。你想想看，一个系统好不好，看轨迹就知道了——平滑还是抖动、有没有漂移、回环检测有没有起作用，全写在轨迹里。

我个人建议，刚开始接触SLAM的同学，先把轨迹可视化玩熟。用 evo 跑几条公开数据集的轨迹，看看真实轨迹和估计轨迹的差异，比看十篇论文都管用。