4. 相对轨迹误差（RPE）：定义、计算方法与ATE的区别
好，咱们今天来聊聊相对轨迹误差，也就是RPE。
说实话，在SLAM领域混久了，你会发现ATE和RPE就像一对双胞胎。很多人一开始搞不清楚它俩到底有啥区别。我当年刚入行的时候也犯过迷糊，觉得「反正都是算误差，用哪个不一样？」
后来踩了坑才明白——ATE看的是全局一致性，RPE看的是局部平滑性。这两个指标，缺一不可。
4.1 RPE到底在算什么？
先给个直观理解。
ATE是拿你估计的轨迹和真值轨迹直接对齐，然后算每个点的位置差。但RPE不一样——它关心的是「相邻两帧之间的相对运动」准不准。
你想想看，一个SLAM系统如果局部漂移很大，哪怕全局看起来还行（比如回环修正后），那它在短时间内的运动估计也是不可靠的。RPE就是用来揪出这种问题的。
RPE的核心思想：
比较估计轨迹和真值轨迹中，相同时间间隔内的相对位姿变化。
说白了，就是看你的里程计在短时间内的「步长」和「转向」准不准。
4.2 数学定义与计算方法
好，咱们上点公式。别怕，很直观。
假设我们有一个时间戳序列 \( t_1, t_2, ..., t_n \)。
对于某个固定的时间间隔 \(\Delta\)（比如1秒、10帧），我们取一对位姿：
- 估计轨迹中：\( P_{est}(t_i) \) 和 \( P_{est}(t_{i+\Delta}) \)
- 真值轨迹中：\( P_{gt}(t_i) \) 和 \( P_{gt}(t_{i+\Delta}) \)
然后计算各自的相对变换：
相对估计变换：T_est_rel = P_est(t_i)^{-1} * P_est(t_{i+Δ})
相对真值变换：T_gt_rel = P_gt(t_i)^{-1} * P_gt(t_{i+Δ})接着，比较这两个相对变换的差异：
误差矩阵：E = T_gt_rel^{-1} * T_est_rel最后，从误差矩阵中提取出平移误差和旋转误差：
RPE_trans = ||trans(E)|| (平移部分的欧氏距离)
RPE_rot = |angle(E)| (旋转部分的角度，通常用弧度或度)嗯，这里要注意：RPE通常是对所有可能的 \((i, i+\Delta)\) 对计算后取均值或RMSE。
我个人习惯：在评估视觉SLAM时，我一般会同时看 \(\Delta=1\) 帧和 \(\Delta=10\) 帧的RPE。前者反映瞬时精度，后者反映短时漂移。
4.3 代码实现示例
光讲理论不过瘾，咱们直接上代码。这是我在项目中常用的RPE计算片段：
import numpy as np
from scipy.spatial.transform import Rotation as R
def compute_rpe(poses_est, poses_gt, delta=1):
"""
poses_est, poses_gt: Nx4x4 的变换矩阵
delta: 时间间隔（帧数）
"""
n = len(poses_est)
errors_trans = []
errors_rot = []
for i in range(n - delta):
# 计算相对变换
T_est_rel = np.linalg.inv(poses_est[i]) @ poses_est[i+delta]
T_gt_rel = np.linalg.inv(poses_gt[i]) @ poses_gt[i+delta]
# 误差矩阵
E = np.linalg.inv(T_gt_rel) @ T_est_rel
# 平移误差
trans_error = np.linalg.norm(E[:3, 3])
errors_trans.append(trans_error)
# 旋转误差
rot_mat = E[:3, :3]
rot = R.from_matrix(rot_mat)
rot_error = np.degrees(rot.magnitude()) # 转成度
errors_rot.append(rot_error)
return np.mean(errors_trans), np.mean(errors_rot)这段代码我用了好几年，基本没出过问题。唯一要注意的是——确保你的位姿矩阵是严格正交的。我曾经因为一个归一化没做好，算出来的RPE忽大忽小，排查了半天才发现是旋转矩阵的数值误差。
4.4 RPE与ATE的核心区别
好，这是本章的重点。咱们用一张表说清楚：
| 对比维度 | ATE（绝对轨迹误差） | RPE（相对轨迹误差） |
|---|---|---|
| 关注点 | 全局一致性 | 局部平滑性 |
| 计算方式 | 先对齐轨迹，再逐点算差 | 不全局对齐，只算相对运动 |
| 对回环的敏感性 | 高（回环修正后ATE会骤降） | 低（主要反映里程计本身） |
| 典型用途 | 评估整体建图精度 | 评估里程计/前端精度 |
| 单位 | 米（平移） | 米/秒 或 度/秒（取决于Δ） |
我再补充一点个人体会：
ATE漂亮不代表RPE漂亮。 我遇到过一些SLAM系统，回环检测做得很好，ATE看起来很低，但RPE一塌糊涂——因为它的前端里程计其实很烂，全靠回环硬拉回来。这种系统在实际应用中是很危险的，因为一旦回环失败，轨迹就飞了。
避坑指南：我曾经评估一个视觉惯性系统时，只看ATE觉得「哇，真准」，结果部署到机器人上后，发现它在转弯时频繁抖动。后来一查RPE，发现旋转部分的误差大得离谱。所以我现在评估任何SLAM系统，ATE和RPE必须同时看，缺一不可。
4.5 知识体系结构图
下面这张图帮你理清RPE在整个轨迹评估中的位置：
4.6 什么时候该用RPE？
我总结了几种典型场景：
- 评估纯里程计：比如你写了一个视觉里程计前端，想看看它漂移大不大。这时候RPE是首选，因为ATE需要全局对齐，会掩盖局部漂移。
- 对比不同前端方案：比如ORB特征 vs. 光流法，用RPE能更公平地比较它们的局部精度。
- 诊断系统瓶颈：如果ATE差但RPE好，说明问题可能出在回环或全局优化上；如果RPE也差，那前端本身就有问题。
一个小技巧：我习惯把RPE画成时间序列图，而不是只给一个均值。这样能直观看到「哪个时间段漂移最大」——往往是转弯或者快速运动的时候。
4.7 本章小结
好，咱们把RPE的核心要点再过一遍：
- RPE衡量的是局部相对运动的精度，不是全局位置精度
- 计算时不需要全局对齐，直接比较相邻帧的相对变换
- 与ATE互补：ATE看全局，RPE看局部，两个一起用才能全面评估
- 时间间隔Δ的选择很关键，一般建议多试几个值
嗯，RPE这块就讲到这里。记住一句话：ATE告诉你「你离目标有多远」，RPE告诉你「你每一步走得稳不稳」。两者都重要，别偏科。
可可精品资料库