1. 轨迹评估基础：为什么需要评估轨迹？
做SLAM这么多年，我经常被问到这样一个问题：
“我的算法跑出来的轨迹，到底准不准？”
说实话，这个问题比想象中要复杂。你想想看，一个机器人在地图里走了几百米，你拿什么去衡量它走得对不对？光靠肉眼盯着屏幕看，那肯定不行。我早期做项目时，就吃过这个亏——看着轨迹挺平滑，结果一上真机就撞墙。
1.1 为什么需要评估轨迹？
说白了，轨迹评估就是给SLAM系统打分。没有这个分数，你根本不知道自己的算法是进步了还是退步了。
我个人习惯把评估目的归纳为三点：
- 量化精度：你的定位误差到底是多少厘米？能不能满足应用需求？
- 对比算法：换了新方案，效果是变好了还是变差了？
- 发现漂移：轨迹在哪个时间段开始跑偏？为什么跑偏？
我记得有一次做AGV项目，客户要求定位误差不超过5厘米。我们跑了十几组数据，用评估工具一算，发现大部分场景都达标，但有一个长走廊场景误差飙到了15厘米。后来排查发现，是走廊纹理太少导致视觉SLAM退化。如果没有轨迹评估，这种问题根本发现不了。
核心观点：没有评估，就没有改进。轨迹评估是SLAM开发的“眼睛”。
1.2 绝对轨迹误差（ATE）
ATE，全称是Absolute Trajectory Error。它衡量的是什么呢？
简单说，就是估计轨迹和真实轨迹之间的整体偏差。你有一个估计的位姿序列，还有一个真值（比如动捕系统或高精度激光雷达给的轨迹）。ATE会先把两条轨迹对齐（消除坐标系差异），然后逐帧计算位置误差。
公式长这样：
ATE = sqrt( 1/N * Σ ||trans(est_i) - trans(gt_i)||² )其中trans(est_i)是第i帧的估计位置，trans(gt_i)是真值位置。说白了就是均方根误差（RMSE）。
ATE的特点：
- 反映全局一致性
- 对漂移敏感——轨迹越长，ATE通常越大
- 适合评估整体定位精度
我的经验：ATE适合做最终指标汇报。给老板看、给客户看，ATE最直观。但要注意，ATE对轨迹长度很敏感，不同长度的轨迹不能直接比ATE数值。
1.3 相对轨迹误差（RPE）
RPE，Relative Pose Error。这个指标关注的是局部一致性。
什么意思呢？它不关心你整体偏了多少，而是关心你每走一段距离（或每过一段时间）的局部误差有多大。
公式：
RPE = sqrt( 1/M * Σ || (est_i⁻¹ * est_j)⁻¹ * (gt_i⁻¹ * gt_j) ||² )其中i和j是间隔固定步长Δ的两帧。这个误差包含了旋转和平移两部分。
RPE的特点：
- 反映局部平滑性
- 对漂移不敏感（因为只看短时间内的相对变化）
- 适合评估里程计性能
避坑指南：我曾经犯过一个错误——只用ATE评估里程计，结果ATE看起来不错，但实际控制时机器人抖动得很厉害。后来才发现，ATE好不代表局部平滑，RPE才是衡量里程计稳定性的关键指标。两个指标要配合使用。
1.4 ATE vs RPE：什么时候用哪个？
| 对比维度 | ATE | RPE |
|---|---|---|
| 关注点 | 全局一致性 | 局部一致性 |
| 对漂移敏感度 | 高 | 低 |
| 适用场景 | SLAM整体精度评估 | 里程计/局部定位评估 |
| 典型应用 | 建图质量、回环检测效果 | 运动平滑性、控制稳定性 |
| 计算方式 | 先对齐再逐帧比较 | 计算相对位姿误差 |
我个人习惯的做法是：
- 做SLAM系统：ATE为主，RPE为辅
- 做纯里程计：RPE为主，ATE为辅
- 做定位漂移修复：两个都要看，ATE看整体漂移量，RPE看局部抖动情况
1.5 知识体系框架
下面这张图是我自己整理的轨迹评估知识结构，你可以对照着看：
1.6 实际项目中的建议
嗯，这里我要多说几句。很多初学者拿到评估工具，直接跑一下出个数字就完事了。但实际项目中，数字背后藏着很多信息。
我建议你这样做：
- 先看ATE：整体精度是否达标？如果ATE太大，说明系统存在严重漂移或回环检测失败。
- 再看RPE：局部抖动是否在可接受范围？如果RPE大但ATE小，说明回环修正后局部仍有跳变。
- 可视化轨迹：把估计轨迹和真值画在一起，肉眼观察偏差区域。我习惯用evo工具，很方便。
- 分段分析：把轨迹按场景分段（直道、弯道、长走廊、开阔区域），分别计算ATE和RPE。这样能定位问题出在哪个环节。
一句话总结：ATE看“准不准”，RPE看“稳不稳”。两个指标缺一不可。
好了，这一章的内容就到这里。轨迹评估是SLAM开发的基石，把ATE和RPE搞明白，后面讲漂移修复时你才能判断修复效果好不好。