一、轨迹评估概述
各位同学好，我是你们这门课的主讲。今天咱们聊聊轨迹评估。说实话，我刚入行那会儿，觉得能跑通SLAM就很牛了，压根没想过还要评估轨迹。直到有一次，我在一个AGV项目里，明明定位感觉挺准的，结果车子就是撞墙——后来一查，轨迹误差大了去了。嗯，从那以后，我再也不敢轻视轨迹评估了。
1.1 什么是轨迹评估
轨迹评估，说白了就是拿你算法算出来的轨迹，跟真实轨迹做对比。你想想看，SLAM系统跑完一圈，输出了一条路径。这条路径到底准不准？误差有多大？光靠肉眼盯着屏幕看，是看不出来的。
我个人的习惯是，把轨迹评估当成一面镜子。它能照出你算法的真实水平。具体来说，我们拿算法估计的位姿序列，和地面真值（Ground Truth）做比较。地面真值怎么来？高精度激光、运动捕捉系统、或者仿真环境里直接拿到的真值。
核心概念：轨迹评估 = 估计轨迹 vs 真实轨迹 的偏差量化分析
1.2 为什么需要轨迹评估
你可能要问：我算法跑起来看着挺稳的，为什么非要做评估？我遇到过不少同学，算法在仿真里跑得飞起，一到真实场景就崩了。为什么？因为没有做系统的轨迹评估。
轨迹评估的作用，我总结为三点：
- 量化精度：你到底有多准？不能说"大概还行"，得拿出数字来。比如ATE 0.05米，RPE 0.02米/秒，这才叫量化。
- 定位问题：误差是累积的？还是突发的？是旋转漂移了？还是平移偏了？评估结果能告诉你问题出在哪。
- 对比算法：你新写的算法比ORB-SLAM3好多少？没有评估，你说服不了别人，也说服不了自己。
我的经验：有一次我在做视觉惯性导航，感觉效果不错。结果一评估，发现旋转误差特别大。后来查了半天，是IMU和相机的标定出了问题。如果没有轨迹评估，我可能永远发现不了这个坑。
1.3 评估指标简介：ATE 与 RPE
轨迹评估的指标有很多，但最常用的就两个：ATE 和 RPE。我刚开始学的时候，也搞混过。这里给你讲清楚。
ATE（绝对轨迹误差）
ATE衡量的是整条轨迹的全局一致性。说白了，就是看你的轨迹和真实轨迹在空间上差了多少。计算方法是：先把估计轨迹和真实轨迹对齐（通常用Sim(3)或SE(3)对齐），然后逐帧计算位姿差。
公式长这样：
ATE = sqrt( 1/N * Σ ||trans( T_gt^{-1} * T_est )||^2 )其中trans()取平移部分。ATE的单位是米，越小越好。
注意：ATE对全局漂移很敏感。如果你的轨迹整体偏了，ATE会很大。但有时候局部精度很好，只是全局有漂移——这时候ATE大，不代表你的算法完全不能用。
RPE（相对位姿误差）
RPE衡量的是局部精度。它看的是固定时间间隔（比如1秒、5秒）内的相对运动误差。这个指标对漂移不那么敏感，更适合评估短时间内的定位稳定性。
公式：
RPE = sqrt( 1/M * Σ ||trans( (T_gt(t)^-1 * T_gt(t+Δt))^-1 * (T_est(t)^-1 * T_est(t+Δt)) )||^2 )RPE的单位可以是米（平移）或度（旋转）。
| 指标 | 衡量什么 | 单位 | 对漂移敏感度 | 典型应用 |
|---|---|---|---|---|
| ATE | 全局轨迹一致性 | 米 | 高 | 闭环检测、全局优化 |
| RPE | 局部运动精度 | 米/度 | 低 | 里程计、局部VIO |
避坑指南：我曾经在一个项目中只看了ATE，觉得效果不错。结果实际部署时，机器人频繁抖动。后来一查RPE，发现局部误差很大。所以我的建议是：两个指标都要看，ATE看大局，RPE看细节。
1.4 知识体系总览
为了让你对本章内容有个整体印象，我画了一张图。这张图展示了轨迹评估的核心逻辑：从原始数据到评估结果，每一步都环环相扣。
这张图你看懂了吗？从左到右，先是输入估计轨迹和真实轨迹，然后做对齐，接着分别计算ATE和RPE，最后输出评估报告。每一步都有讲究，后面几章我会带你手把手实操。
一句话总结：轨迹评估不是可有可无的锦上添花，而是SLAM系统开发中不可或缺的一环。ATE看全局，RPE看局部，两个指标配合使用，才能全面评估你的定位精度。
好了，这一章就到这里。记住我今天说的：别光看轨迹跑得漂亮，拿数据说话才是硬道理。下一章，我会带你实际动手，用evo工具跑一遍轨迹评估，看看你的算法到底几斤几两。