4. 相对轨迹误差（RPE）：定义、计算方法与ATE的区别
聊完绝对轨迹误差（ATE），咱们来看看另一个核心指标——相对轨迹误差（RPE）。
说实话，我刚入行那会儿，总觉得ATE就够了。直到有一次做视觉SLAM的闭环检测评估，ATE显示误差很小，但实际建图就是不对劲。后来才发现，问题出在局部轨迹的抖动上。嗯，这时候RPE就派上用场了。
4.1 RPE到底在衡量什么？
RPE，全称Relative Pose Error。它衡量的是轨迹中相邻（或固定间隔）位姿之间的相对变换误差。
你想想看，ATE看的是整体轨迹的漂移，但RPE看的是局部一致性。说白了，就是检查你的算法在短时间内的运动估计准不准。
核心思想：RPE计算的是真实位姿变化与估计位姿变化之间的差异。
公式表达：
E_i = (Q_i^{-1} * Q_{i+Δ})^{-1} * (P_i^{-1} * P_{i+Δ})其中：
- P_i, P_{i+Δ}：估计轨迹中的两个位姿
- Q_i, Q_{i+Δ}：真实轨迹中的两个位姿
- Δ：时间间隔（步长）
4.2 RPE的计算方法
计算RPE其实不复杂，我一般分三步走：
- 对齐轨迹——和ATE一样，先做时间戳对齐和坐标系对齐
- 计算相对变换——对每个时间步，计算真实轨迹和估计轨迹各自的相对位姿变化
- 求误差——比较这两个相对变换的差异
这里有个关键点：步长Δ怎么选？
我个人习惯的做法是：
- 评估里程计漂移：Δ=1（相邻帧）
- 评估闭环检测：Δ取较大值（比如10帧或100帧）
- 全面评估：计算多个Δ下的RPE，取平均值
我的经验：在项目中，我通常会把Δ从1遍历到轨迹长度的10%，然后绘制RPE随Δ变化的曲线。这样能直观看到算法在不同时间尺度下的表现。
4.3 RPE与ATE的核心区别
这个问题我经常被问到。咱们直接上对比表：
| 对比维度 | ATE（绝对轨迹误差） | RPE（相对轨迹误差） |
|---|---|---|
| 衡量对象 | 全局位姿的绝对偏差 | 局部相对运动的偏差 |
| 对初始对齐敏感 | 是（需要对齐坐标系） | 否（相对量，天然对齐） |
| 反映的问题 | 整体漂移、闭环效果 | 局部抖动、里程计精度 |
| 典型应用场景 | SLAM建图质量评估 | VIO/里程计性能评估 |
| 计算复杂度 | 较低（一次对齐） | 较高（逐帧计算） |
为什么会这样？我举个例子你就明白了。
假设你开车从A点到B点：
- ATE关心的是：你最终停在了B点附近吗？
- RPE关心的是：你每段路的行驶轨迹是否平滑？有没有来回打方向盘？
我曾经在一个视觉惯性里程计项目中，ATE表现很好（最终位置误差<5cm），但RPE却很大。一查原因，是IMU的零偏估计不稳定，导致每帧之间的旋转估计有抖动。这种问题，只看ATE根本发现不了。
避坑指南：千万不要只用ATE来评估SLAM系统！
我曾经见过一个团队，ATE指标刷得很漂亮，但实际跑起来地图就是飘。后来发现他们的闭环检测虽然能纠正全局漂移，但局部轨迹的RPE高达0.5m/s。这种系统在自动驾驶里根本不能用——你想想看，每秒钟定位偏差半米，控制模块怎么跟得上？
4.4 什么时候该用RPE？
根据我的经验，以下场景RPE是必选项：
- 评估VIO/里程计——没有闭环，ATE会持续增长，RPE才是真实性能
- 对比不同传感器融合方案——比如纯视觉vs视觉+IMU，RPE能看出局部稳定性差异
- 调试参数——调整某个参数后，RPE的变化往往比ATE更敏感
- 实时性要求高的系统——比如自动驾驶，局部轨迹的平滑度直接影响控制
4.5 一个实用的计算示例
这里给一段我常用的Python代码片段，用于计算RPE：
import numpy as np
from scipy.spatial.transform import Rotation as R
def compute_rpe(est_poses, gt_poses, delta=1):
"""
计算相对轨迹误差
est_poses, gt_poses: Nx4x4 的变换矩阵
delta: 时间步长
"""
n = len(est_poses)
errors = []
for i in range(n - delta):
# 估计轨迹的相对变换
est_rel = np.linalg.inv(est_poses[i]) @ est_poses[i + delta]
# 真实轨迹的相对变换
gt_rel = np.linalg.inv(gt_poses[i]) @ gt_poses[i + delta]
# 误差 = 真实相对变换的逆 * 估计相对变换
error = np.linalg.inv(gt_rel) @ est_rel
# 提取平移误差（米）
trans_error = np.linalg.norm(error[:3, 3])
# 提取旋转误差（度）
rot_error = R.from_matrix(error[:3, :3]).magnitude() * 180 / np.pi
errors.append([trans_error, rot_error])
return np.array(errors)
# 使用示例
rpe_results = compute_rpe(estimated_trajectory, ground_truth_trajectory, delta=10)
print(f"平均平移RPE: {np.mean(rpe_results[:, 0]):.3f} m")
print(f"平均旋转RPE: {np.mean(rpe_results[:, 1]):.3f} deg")小技巧：实际项目中，我建议同时计算平移RPE和旋转RPE。有时候平移误差小但旋转误差大，说明算法在角度估计上有问题。反过来也一样。
4.6 知识体系总结
为了让你更直观地理解RPE在整个轨迹评估中的位置，我画了一张图：
这张图很清楚地展示了ATE和RPE各自的定位。我个人的建议是：评估SLAM系统时，两个指标都要看。ATE告诉你最终结果好不好，RPE告诉你过程稳不稳。
最后说一句：RPE的计算虽然比ATE复杂一点，但它能暴露的问题往往更隐蔽、更致命。下次评估你的算法时，别忘了把RPE加上。
可可精品资料库