轨迹评估指标深度解析 - 第1节
一、轨迹评估概述：为什么需要轨迹评估？
做SLAM这么多年，我经常被问到同一个问题：“你凭什么说你这个算法比别人的好？”
嗯，这个问题其实挺难回答的。你总不能说“我觉得我的轨迹更平滑”吧？那太主观了。我们需要一个客观的、可量化的标准——这就是轨迹评估存在的意义。
说白了，轨迹评估就是一把尺子。它用来衡量我们估计出来的机器人轨迹，到底有多接近真实轨迹。没有这把尺子，你根本没法判断你的算法是进步了还是退步了。
我在项目中遇到过不少这样的情况：明明感觉算法调优后效果变好了，结果一跑评估指标，发现某些指标反而更差了。这说明什么？说明感觉会骗人，但数据不会。
为什么非做不可？
我总结下来，主要有三个原因：
- 量化对比：不同算法之间谁优谁劣，不能光靠嘴说。有了统一的评估指标，才能公平地比较。
- 定位问题：你的算法在哪个环节出了问题？是前端匹配漂移了，还是后端优化没收敛？轨迹评估能帮你把问题揪出来。
- 论文说服力：写论文时，没有评估结果，审稿人直接拒稿。这是硬通货。
核心观点：轨迹评估不是锦上添花，而是SLAM系统的“体检报告”。没有它，你根本不知道你的系统健康不健康。
评估指标的分类：绝对指标 vs 相对指标
评估指标听起来很多，什么ATE、RPE、RMSE……但其实它们可以归为两大类。我个人习惯把它们分成绝对指标和相对指标。这两类指标看问题的角度完全不同。
绝对指标（Absolute Metrics）
绝对指标，顾名思义，就是看你的轨迹和真实轨迹在全局坐标系下的差异。它关心的是：你估计出来的整个轨迹，和真实轨迹到底有多像？
最典型的代表就是ATE（Absolute Trajectory Error，绝对轨迹误差）。
ATE的计算思路很简单：先把估计轨迹和真实轨迹对齐（通常用最小二乘法做刚体变换），然后逐帧计算位置误差。最后统计这些误差的均值、中位数、RMSE等。
你想想看，ATE其实是在回答一个问题：“你整体上跑偏了多远？”
我的经验：ATE对全局一致性要求很高。如果你的算法在长时间运行后出现了明显的漂移，ATE会立刻暴露出来。我曾经在一个大场景数据集上跑ORB-SLAM2，ATE的RMSE从0.3米飙到了1.2米，一看就知道后端回环检测没起作用。
相对指标（Relative Metrics）
相对指标则关注局部一致性。它不关心你整体偏了多少，而是关心你每走一段距离，误差有多大。
最典型的代表是RPE（Relative Pose Error，相对位姿误差）。
RPE的计算方法是：在轨迹上取固定长度的一段（比如1米、5米、10米），计算这段距离内估计位姿变化和真实位姿变化的差异。然后滑动窗口，统计所有段的误差。
说白了，RPE回答的是：“你每走一步，误差积累了多少？”
避坑指南：我曾经犯过一个错误——只看ATE，不看RPE。结果ATE看起来不错，但实际跑起来机器人抖动得很厉害。后来一查RPE，发现局部误差很大，只是全局对齐后把误差“抹平”了。所以，绝对指标和相对指标必须一起看，缺一不可。
两类指标的对比
| 维度 | 绝对指标（ATE） | 相对指标（RPE） |
|---|---|---|
| 关注点 | 全局一致性 | 局部一致性 |
| 典型应用 | 评估整体漂移 | 评估局部抖动 |
| 对回环的敏感性 | 高（回环能大幅改善ATE） | 低（回环对局部影响有限） |
| 计算方式 | 先对齐，再逐帧比较 | 分段比较位姿变化量 |
| 常用统计量 | RMSE、均值、中位数 | RMSE、均值、中位数 |
知识体系结构图
下面这张图，是我自己梳理的轨迹评估知识体系。你可以把它当作本章的“地图”。
一个简单的代码示例
这里我用Python演示一下ATE和RPE的核心计算逻辑。注意，这只是示意代码，实际工程中建议用成熟的库（比如evo）。
import numpy as np
# 模拟数据：真实轨迹和估计轨迹（N x 3，位置）
gt_traj = np.random.randn(100, 3) # 真实轨迹
est_traj = gt_traj + np.random.randn(100, 3) * 0.1 # 估计轨迹（加噪声）
# ---------- ATE 计算 ----------
# 1. 对齐（这里简化，实际用最小二乘）
# 2. 逐帧计算位置误差
ate_errors = np.linalg.norm(est_traj - gt_traj, axis=1)
ate_rmse = np.sqrt(np.mean(ate_errors ** 2))
print(f"ATE RMSE: {ate_rmse:.4f} m")
# ---------- RPE 计算 ----------
delta = 10 # 步长间隔
rpe_errors = []
for i in range(len(gt_traj) - delta):
# 计算真实轨迹的位姿变化
gt_delta = gt_traj[i + delta] - gt_traj[i]
# 计算估计轨迹的位姿变化
est_delta = est_traj[i + delta] - est_traj[i]
# 计算变化量的差异
error = np.linalg.norm(est_delta - gt_delta)
rpe_errors.append(error)
rpe_rmse = np.sqrt(np.mean(np.array(rpe_errors) ** 2))
print(f"RPE RMSE (delta={delta}): {rpe_rmse:.4f} m")
小技巧：实际使用中，RPE的delta参数很关键。我一般会尝试多个delta值（比如1m、5m、10m），然后看曲线趋势。如果delta增大时RPE增长很快，说明你的算法存在严重的累积误差问题。
总结一下
轨迹评估这件事，说白了就是给你的SLAM算法做“体检”。绝对指标看整体，相对指标看局部。两者结合，才能全面了解算法的真实水平。
嗯，这一章就到这里。记住：没有评估，就没有进步。