1. 轨迹评估概述
大家好，我是你们的老朋友。今天咱们聊聊轨迹评估——这个在SLAM和自动驾驶里绕不开的话题。
说实话，我刚开始做SLAM那会儿，对轨迹评估的理解特别浅。觉得算法跑起来，地图建得漂亮，定位看着准，那就行了。直到有一次，我在一个园区项目里，算法在大部分场景下表现都很好，但到了某个特定路口，定位突然跳了半米。要不是做了轨迹评估，我根本发现不了这个问题。嗯，从那以后，我再也不敢轻视轨迹评估了。
什么是轨迹评估
轨迹评估，说白了就是拿你算法估计出来的轨迹，跟真实轨迹做对比。真实轨迹我们通常叫它「真值」（Ground Truth）。
你想想看，SLAM系统或者自动驾驶的定位模块，输出的是一个时间序列的位姿。这个位姿序列就是轨迹。但问题是，我们怎么知道它准不准？
答案很简单：找个更准的东西来比。
真值怎么来？常见的有这么几种：
- 高精度惯导+RTK：厘米级精度，室外场景首选
- 运动捕捉系统：室内场景，精度毫米级
- 仿真器：比如CARLA、Gazebo，真值直接从仿真环境拿
- 手动标注：比如用激光扫描仪建好的地图做参考
我个人习惯，能用高精度惯导就用惯导。为什么？因为省心。你不需要额外处理时间同步、坐标系对齐这些麻烦事。当然，价格也贵，一套下来十几万很正常。
核心要点：轨迹评估的本质就是「比对」。比对之前，你得先解决两个问题：时间对齐和坐标系对齐。这两个坑，我踩过不止一次。
为什么需要轨迹评估
这个问题，我问过不少刚入行的朋友。他们的回答大多是「为了看算法好不好」。这个答案没错，但太笼统了。
我总结了一下，轨迹评估至少有三个层面的价值：
- 量化算法性能：你的算法到底有多准？不能说「感觉还行」，得拿出数字来。ATE 0.1米还是0.5米，差很多。
- 定位问题根因：算法在哪些场景下表现不好？是转弯的时候漂了，还是高速的时候跟丢了？轨迹评估能帮你找到薄弱环节。
- 对比不同方案：你用了A算法，我用了B算法，谁更好？别争，跑一遍评估，看指标说话。
我记得有一次，我们团队开发了一个新的VIO算法。在公开数据集上跑，指标看着不错。但放到自己的采集车上，效果差了一大截。后来一查，发现是IMU和相机的标定参数有问题。如果没有轨迹评估，这个问题可能要到路测阶段才能暴露出来。
注意：轨迹评估不是万能的。它只能告诉你「准不准」，不能告诉你「为什么不准」。定位问题的根因分析，需要结合其他手段，比如可视化、日志分析等。
评估指标：ATE 和 RPE
说到指标，最常用的就是两个：ATE（绝对轨迹误差）和 RPE（相对位姿误差）。
这两个指标，我刚开始也搞混过。后来想明白了一个道理：ATE看全局，RPE看局部。
ATE（绝对轨迹误差）
ATE衡量的是估计轨迹和真实轨迹之间的整体偏差。说白了，就是看你的轨迹整体上跟真值有多像。
计算方式很简单：先把两条轨迹对齐（通常用Umeyama算法做相似变换），然后逐帧计算位置误差。
# 伪代码示例
for each timestamp t:
error_t = ||p_est_t - p_gt_t||
ATE = sqrt(mean(error_t^2))ATE的单位通常是米。值越小，说明整体定位越准。
我在项目中遇到过一个问题：ATE看起来很好，但局部抖动很厉害。这就是ATE的局限性——它会把误差平均掉。所以光看ATE是不够的。
RPE（相对位姿误差）
RPE衡量的是轨迹的局部一致性。它计算的是固定时间间隔内，位姿变化的误差。
举个例子：你每隔1秒算一次位姿变化，然后跟真值的位姿变化做对比。这个误差就是RPE。
# 伪代码示例
delta = 1 # 时间间隔，单位秒
for each timestamp t:
delta_est = p_est_t * inv(p_est_{t-delta})
delta_gt = p_gt_t * inv(p_gt_{t-delta})
error_t = ||delta_est - delta_gt||
RPE = sqrt(mean(error_t^2))RPE的单位也是米。它反映的是轨迹的平滑性和局部精度。如果你的算法有漂移，RPE会明显变大。
我的建议：评估一个SLAM系统，ATE和RPE都要看。ATE告诉你整体准不准，RPE告诉你局部稳不稳。两个指标都好的算法，才是好算法。
两个指标的工程意义
咱们从工程落地的角度，聊聊这两个指标到底意味着什么。
| 指标 | 工程意义 | 典型场景 |
|---|---|---|
| ATE | 全局定位精度，决定车辆能否在全局地图中正确定位 | 自动驾驶中的车道级定位、AR中的虚拟物体放置 |
| RPE | 局部运动平滑度，决定控制模块能否稳定执行 | 路径规划、轨迹跟踪、避障 |
举个例子你就明白了。自动驾驶里，如果ATE差，车可能跑错车道。如果RPE差，车可能开起来一抖一抖的，乘客体验极差。
我曾经在一个项目中，ATE做到了0.1米以内，但RPE有0.05米。看起来不错对吧？但实际路测的时候，车辆在转弯时方向盘频繁修正，坐车的同事都晕车了。后来我们把RPE优化到0.02米以下，问题才解决。
总结一下：ATE和RPE不是二选一，而是相辅相成。做工程落地，两个指标都要盯。我个人习惯，先看ATE，确保整体精度达标；再看RPE，优化局部平滑性。
知识体系总览
下面这张图，是我自己整理的轨迹评估知识体系。你可以把它当作本章的思维导图。
这张图把轨迹评估的核心内容串起来了。从定义到意义，再到具体指标，一目了然。你可以把它当作一个快速参考。
一个小技巧：做轨迹评估的时候，别只看最终的数字。把误差随时间变化的曲线画出来，往往能发现更多问题。比如某个时间段误差突然变大，那大概率是那个时间段的数据有问题。
好了，这一章的内容就到这里。轨迹评估是SLAM和自动驾驶里最基础也最重要的环节之一。搞懂了它，你后面的工作会顺利很多。
可可精品资料库