1. 轨迹评估概述
各位同学好，我是你们的老朋友。今天咱们聊聊轨迹评估。
说实话，我刚入行那会儿，对轨迹评估这事儿挺不以为然的。觉得算法跑起来就行，评估不评估的，差不多得了。结果呢？有一次在AGV项目上，算法在仿真里跑得飞起，一上真车就歪歪扭扭。折腾了三天，最后发现是轨迹评估没做细，一个微小的漂移没抓住。嗯，从那以后，我再也不敢小看轨迹评估了。
什么是轨迹评估？
说白了，轨迹评估就是拿你算法算出来的轨迹，跟真实轨迹做对比。看看差了多少，差在哪里。
你想想看，SLAM系统输出的是什么？是一串位姿序列。每个位姿包含位置和姿态。但问题是，我们怎么知道这个位姿准不准？
答案就是：找个“标准答案”来比。
这个“标准答案”，我们叫它真值轨迹。可以是高精度激光跟踪仪测的，也可以是动作捕捉系统给的。总之，它得比你的算法结果准一个数量级以上。
核心定义：轨迹评估 = 估计轨迹 vs 真值轨迹 的偏差量化分析
为什么需要轨迹评估？
我遇到过不少新手，觉得算法能跑就行。其实这是个坑。
轨迹评估至少有三个作用：
- 量化性能——你说你的算法好，好多少？拿数据说话。
- 定位问题——是整体漂移？还是局部抖动？评估结果能告诉你。
- 对比方案——不同算法、不同参数，哪个更优？一评估便知。
我记得有一次做室内机器人定位，换了三种前端匹配策略。肉眼看着都差不多，一算ATE，差距就出来了。最好的和最差的，差了将近40%。你说，不评估行吗？
避坑提醒：我曾经犯过一个错误——只评估了整体误差，没看局部。结果整体误差看着还行，但机器人转弯时抖动严重。后来加上RPE分析，才把问题揪出来。所以，评估一定要全面。
评估指标分类
轨迹评估的指标，主要分两大类。我习惯这么记：一个看“全局准不准”，一个看“局部稳不稳”。
绝对轨迹误差（ATE）
ATE，全称Absolute Trajectory Error。它衡量的是整条轨迹的全局一致性。
怎么算的呢？先把估计轨迹和真值轨迹对齐（通常用Umeyama算法做刚体变换），然后逐帧计算位姿差。
公式长这样：
ATE = sqrt( 1/N * Σ ||trans(est_i) - trans(gt_i)||² )其中trans()取的是平移部分。当然，也可以算旋转误差，但实践中平移误差更常用。
我个人习惯用RMSE作为ATE的最终指标。为什么？因为它对大误差敏感。你想想看，如果轨迹在某处突然跳变一下，RMSE会立刻反映出来。
| 指标 | 含义 | 适用场景 |
|---|---|---|
| ATE RMSE | 全局平移误差的均方根 | 整体精度评估 |
| ATE Mean | 全局平移误差的平均值 | 系统偏差分析 |
| ATE Median | 全局平移误差的中位数 | 抗离群点评估 |
小技巧：我一般会同时看RMSE和Median。如果两者差距很大，说明轨迹里有明显的离群点。这时候就要去排查了。
相对轨迹误差（RPE）
RPE，全称Relative Pose Error。它看的是局部一致性——也就是相邻帧之间的相对位姿准不准。
为什么需要这个？因为ATE只能告诉你“整体偏了多少”，但没法告诉你“是不是每走一步都在抖”。
RPE的计算方式：取一个固定时间间隔Δ，然后计算这个间隔内的相对位姿误差。
RPE = sqrt( 1/M * Σ || (est_i⁻¹ * est_{i+Δ})⁻¹ * (gt_i⁻¹ * gt_{i+Δ}) ||² )嗯，公式看着有点复杂。但说白了，就是看“走一步”的误差有多大。
我在项目中遇到过这样的情况：ATE表现很好，但RPE一塌糊涂。后来发现是IMU和视觉的融合参数没调好，导致每帧之间都有微小抖动。这种问题，光看ATE根本发现不了。
| 指标 | 含义 | 适用场景 |
|---|---|---|
| RPE Translation | 局部平移误差 | 运动平滑性评估 |
| RPE Rotation | 局部旋转误差 | 姿态稳定性评估 |
注意：RPE的Δ选择很关键。Δ太小，反映的是帧间噪声；Δ太大，又接近ATE了。我一般取Δ=1秒对应的帧数，或者根据运动速度动态调整。
知识体系总览
下面这张图，是我自己总结的轨迹评估知识框架。你看一眼，心里就有数了。
你看，整个体系其实很清晰。输入是两条轨迹，经过预处理，然后从两个维度去评估。ATE看全局，RPE看局部。两者结合，才能全面评价一个SLAM系统的性能。
总结一句话：ATE告诉你“偏了多少”，RPE告诉你“抖不抖”。两个都得看，缺一不可。
好了，这一章就到这里。记住，做SLAM，评估和算法本身一样重要。别像我当年那样，吃了亏才长记性。
可可精品资料库