一、轨迹评估概述

1.1 什么是轨迹评估

轨迹评估，说白了就是拿你算出来的轨迹，跟真值去比一比。

我经常跟团队里新人说：你SLAM跑得再花哨，轨迹一评估就露馅了。为什么？因为轨迹是SLAM系统最直接的输出。你算出来的每一帧位姿，到底准不准？漂移了多少？有没有回环？这些都得靠轨迹评估来回答。

具体来说，轨迹评估要做三件事：

• 对齐：把估计轨迹和真值轨迹对齐到同一个坐标系下
• 比较：逐帧或逐段比较位姿差异
• 统计：算出误差指标，比如ATE、RPE这些

嗯，这里要注意。对齐这一步特别关键。我见过不少新手，直接拿两个轨迹的xyz坐标就开算，结果误差大得离谱。其实是因为坐标系没对齐——你估计的轨迹是在SLAM起始坐标系下，真值可能是GPS坐标系或者运动捕捉系统的坐标系。不对齐就比，那是耍流氓。

1.2 为什么需要轨迹评估

你想想看，一个SLAM系统跑完了，你怎么知道它好不好？

看地图漂不漂亮？那太主观了。看能不能回环？那只是定性判断。真正客观的、可量化的评价，就得靠轨迹评估。

我个人习惯把轨迹评估的作用归纳为四点：

1. 量化性能：给出具体的数值，比如ATE的RMSE是0.05米还是0.5米
2. 定位问题：哪一段轨迹漂移严重？是转弯的时候还是直行的时候？
3. 对比算法：你的ORB-SLAM3和我的DROID-SLAM，谁更准？拿数据说话
4. 调参依据：改了一个参数，效果是变好了还是变差了？轨迹评估告诉你

我在项目中遇到过一件事。有一次做室内机器人导航，视觉SLAM跑得挺顺，地图看着也干净。结果一评估轨迹，发现有一段2米的路径，误差累积到了30厘米。后来排查发现是那段路纹理太弱，特征点匹配出了问题。如果没有轨迹评估，这个问题可能要到机器人撞墙了才发现。

1.3 轨迹评估在SLAM中的应用场景

轨迹评估在SLAM里，几乎无处不在。我按自己的经验，给你梳理几个典型场景：

场景	评估重点	常用指标
视觉SLAM算法开发	整体定位精度、漂移情况	ATE RMSE、ATE max
多传感器融合	各传感器贡献度、融合效果	RPE、相对位姿误差
回环检测验证	回环前后轨迹一致性	绝对轨迹误差变化
长时间运行测试	累积漂移趋势	分段ATE、时间序列分析
算法对比评测	公平比较不同算法	多种指标综合对比

在自动驾驶领域，轨迹评估更是命根子。你想想，一辆车在高速上跑，定位误差超过10厘米可能就压线了。所以自动驾驶的轨迹评估，要求更高、更严格。

我曾经参与过一个自动驾驶项目，客户要求定位精度在5厘米以内。我们用了RTK-GPS做真值，然后逐帧评估视觉SLAM的轨迹。结果发现，在隧道里GPS信号丢失的时候，纯视觉定位的误差会迅速累积。后来我们加了IMU和轮速计，才把隧道内的误差控制在要求范围内。

自动驾驶里常见的轨迹评估场景包括：

• 高精地图构建：需要厘米级的轨迹精度
• 感知融合验证：检测目标的位置是否准确，依赖于自车定位
• 规划控制评估：轨迹跟踪误差，看控制器的表现
• 功能安全测试：极端场景下的定位可靠性

1.4 轨迹评估的核心逻辑

说了这么多，我们来画一张图，把轨迹评估的核心逻辑理清楚。

这张图把轨迹评估的流程讲得很清楚了。从估计轨迹和真值轨迹两个输入开始，经过对齐、误差计算、统计指标，最后输出评估报告。每一步都有坑，后面我们会一个一个讲。

1.5 常见的轨迹评估工具

工欲善其事，必先利其器。我常用的轨迹评估工具有这么几个：

• evo：Python写的，轻量好用，支持TUM、KITTI、EuRoC等多种格式
• rpg_trajectory_evaluation：苏黎世理工开源的，功能全面
• MATLAB Robotics Toolbox：做科研论文必备，可视化漂亮
• 自己写脚本：有时候工具不够灵活，我就自己写Python脚本

我个人最常用的是evo。为什么？因为它简单。一行命令就能出结果，还能画图。比如：

# 评估ATE
evo_ape tum groundtruth.txt estimated.txt -va --plot

# 评估RPE
evo_rpe tum groundtruth.txt estimated.txt -va --plot

# 多条轨迹对比
evo_traj tum traj1.txt traj2.txt --ref=groundtruth.txt -va --plot

你看，就这么几行命令，ATE、RPE、轨迹对比全出来了。后面我们会专门用一章来讲evo的使用技巧，包括怎么处理时间戳、怎么对齐坐标系、怎么导出数据做论文图表。

好了，这一章就到这里。轨迹评估的概念、重要性、应用场景和核心流程，你应该有个大概的了解了。下一章我们开始讲具体的评估指标——ATE和RPE，这两个是轨迹评估的基石。