视觉SLAM基础：算法精度评价指标（ATE、RPE）

1. 前言

• 当我们需要评估一个SLAM/VO算法的表现时，可以从时耗、复杂度、精度多个角度切入，其中对精度的评价是我们最关注的，这个过程中我们不可避免会遇到两个精度指标ATE和RPE。这两个evaluation metrics最早是在TUM数据集benchmark中定义的，应用非常广泛。
• 介绍前，先对定义一下公式标注：
  算法估计位姿：$P_1,...,P_n\in SE(3)$
  真实位姿：$Q_1,...,Q_n\in SE(3)$
  下标代表时间$t$（或帧）,这里我们假设估计位姿和真实位姿各帧时间已对齐，总帧数相同。
  $\Delta$表示相隔时间time interval

2. RPE： relative pose error 相对位姿误差

相对位姿误差主要描述的是相隔固定时间差$\Delta$两帧位姿差的精度（相比真实位姿），相当于直接测量里程计的误差。
因此第$i$帧的RPE定义如下：
$$E_i:=(Q^{-1}{Q}_{i+\Delta }{)}^{-1}(P_i^{-1}{P}_{i+\Delta })$$
已知总数$n$与间隔$\Delta$的情况下，可以得到$m=n-\Delta$个RPE，然后我们可以用均方根误差RMSE统计这个误差，得到一个总体值：
$$\text{RMSE}(E_{1:n},\Delta ):=(\frac{1}{m}\sum _{i=1}^m\Vert trans(E_i){\Vert }^2{)}^{\frac{1}{2}}$$
其中$trans(E_i)$代表取相对位姿误差中的平移部分translation。当然也有人不用RMSE，直接使用平均值、甚至中位数来描述相对误差情况。
需要注意的是，RPE包含了两部分的误差，分别是旋转误差和平移误差，通常使用平移误差进行评价已经足够，但是如果需要，旋转角的误差也可以使用相同的方法进行统计。
到这一步，我们基本可以从RMSE值的大小来评价算法的表现，然而实际情况中，我们发现对$\Delta$的选取有多种选择，为了能综合衡量算法表现，我们可以计算遍历所有$\Delta$的RMSE的平均值：
$$\text{RMSE}(E_{1:n}):=\frac{1}{n}\sum _{\Delta =1}^n\text{RMSE}(E_{1:n},\Delta ).$$
但这样新的问题又出现了，这样的计算复杂度非常高，很耗时间，因此TUM在自己给定的工具中，通过计算固定数量的RPE样本计算一个估计值作为最终结果。

3. ATE： absolute trajectory error 绝对轨迹误差

绝对轨迹误差是估计位姿和真实位姿的直接差值，可以非常直观地反应算法精度和轨迹全局一致性。需要注意的是，估计位姿和groundtruth通常不在同一坐标系中，因此我们需要先将两者对其：对于双目SLAM和RGB-D SLAM,尺度统一，因此我们需要通过最小二乘法计算一个从估计位姿到真实位姿的转换矩阵$\text{S}\in SE(3)$；对于单目相机，具有尺度不确定性，我们需要计算一个从估计位姿到真实位姿的相似转换矩阵$\text{S}\in Sim(3)$。
因此第$i$帧的ATE定义如下：
$$F_i:=Q_i^{-1}S{P}_i$$
与RPE相似，建议使用RMSE统计ATE
$$\text{RMSE}(F_{1:n},\Delta ):=(\frac{1}{m}\sum _{i=1}^m\Vert trans(F_i){\Vert }^2{)}^{\frac{1}{2}}$$
当然，使用平均值、中位数等来反应ATE亦可，现在很多evaluation工具会将RMSE、Mean、Median都给出。
综上，我们需要注意的是，RPE误差包含了translation和rotation两部分的误差，而ATE只包含translation的误差（从二者的全称中也可以看出），二者具有强烈的相关性，却也不尽相同。我们仍需结合实际，选择合适的指标进行算法评价。

4. 工具：EVO

这里推荐一个方便全能的evaluation工具。这里贴出github链接
EVO:Python package for the evaluation of odometry and SLAM

4.1 安装

系统：ubuntu，首先确保系统安装了Python，然后根据需要可以选择性安装**PyQt5 / PyQt4 **和 ROS，当然， 不装也不会影响基本功能。

1. 直接从pip安装，不需要查看源代码。

   pip install evo --upgrade --no-binary evo
   

2. 源代码安装

   git clone [email protected]:MichaelGrupp/evo.git
   cd evo
   pip install --editable . --upgrade --no-binary evo
   

3. 输入下面这个命令，如果成功会有响应。

   evo_ate -h
   

4.2 usage

可以先用evo仓库中自带的数据尝试

1. 轨迹可视化

cd test/data
evo_traj kitti KITTI_00_ORB.txt KITTI_00_SPTAM.txt --ref=KITTI_00_gt.txt -p --plot_mode=xz

2. APE

evo_ape kitti KITTI_00_gt.txt KITTI_00_ORB.txt -va --plot --plot_mode xz

3. RPE

evo_rpe tum fr2_desk_groundtrutr2_desk_ORB.txt -va --plot --plot_mode xyz

参数较多，每个命令都有比较详细的帮助，善用 --help，如：evo_ape --help

这里列举了一些常见选项的含义：

选项	含义
-a, --align	轨迹对齐
-s, --correct_scale	尺度对齐
–sync	通过时间戳关联轨迹
-p, --plot	可视化轨迹
-v, --verbose	命令号输出细节信息
–plot_mode	轨迹可视化模式：xyz, xy, xz等
–save_plot	保存plot结果

4.3 metrics解析

上文提到pose error包含平移和旋转两部分误差，在evo中的option为-r，–pose_relation，并有如下模式：

-r {…}	功能
full	同时考虑旋转与平移，无单位。
trans_part	平移误差，单位m。(默认)
rot_part	旋转误差，无单位。
angle_deg	旋转误差，单位degree。
angle_rad	旋转误差，单位弧度rad。

其中trans_part很容易理解，但是无单位的full和rot_part的误差指标比较特殊，使用了矩阵的二范数：

$erro{r}_{full}=\Vert \delta T_i-E\Vert$

其中KaTeX parse error: Undefined control sequence: \deltaT at position 1: \̲d̲e̲l̲t̲a̲T̲_i表示groundtruth和estimate位姿之间的变换矩阵，$E$为单位阵，矩阵的范数也可以表示某种抽象的空间距离，从而用来度量误差（仅供参考，求赐教包括为什么要减去单位阵）。

同理rot_part的误差：

$erro{r}_{rot}=\Vert \delta R_i-E\Vert$

相比较而言，用平移误差单位为m，旋转误差单位可以是度或弧度，更容易理解。从旋转矩阵$R$到旋转角的转换使用了罗德里格斯公式（旋转向量的二范数为旋转角的大小）。

$R=\cos \theta I+(1-\cos \theta )n{n}^T+\sin \theta n^{\wedge }$

从而：

$\theta =\arccos (\frac{traceR-1}{2})$

#附EVO metric的相关代码
if self.pose_relation == PoseRelation.translation_part:
    # E is an array of position vectors only in this case
    self.error = [np.linalg.norm(E_i) for E_i in self.E]
elif self.pose_relation == PoseRelation.rotation_part:
    self.error = np.array([
        np.linalg.norm(lie.so3_from_se3(E_i) - np.eye(3))
        for E_i in self.E])
elif self.pose_relation == PoseRelation.full_transformation:
    self.error = np.array(
        [np.linalg.norm(E_i - np.eye(4)) for E_i in self.E])
elif self.pose_relation == PoseRelation.rotation_angle_rad:
    self.error = np.array(
        [abs(lie.so3_log(E_i[:3, :3])) for E_i in self.E])
elif self.pose_relation == PoseRelation.rotation_angle_deg:
    self.error = np.array([
        abs(lie.so3_log(E_i[:3, :3])) * 180 / np.pi for E_i in self.E
    ])
else:
    raise MetricsException("unsupported pose_relation")

参考

1. Sturm, Jürgen, et al. “A benchmark for the evaluation of RGB-D SLAM systems.” 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2012.
2. evo https://github.com/MichaelGrupp/evo
3. 高翔. 视觉 SLAM 十四讲: 从理论到实践. 电子工业出版社, 2017.