SLAM的精度评价工具——EVO工具箱的使用、原理、缺陷和改进的相关思考

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

EVO是SLAM中常用的精度评价工具之一，其原理来源于《A benchmark for the evaluation of RGB-D SLAM system》，其通过Umeyama原理实现轨迹对齐。EVO工具支持多种数据集的轨迹格式，如TUM、KITTI、EuRoC MAV以及ROS的bag文件，同时支持这些数据格式之间的相互转换；且其提供三种误差评估方式：绝对位姿误差（evo_ape）、相对位姿误差（evo_rpe）以及针对每个子序列的平均位姿误差。然而，目前EVO还存在若干问题，且未得到很好的解决。笔者之前对坐标系转换与精度评定的研究较多，对相关工作有一定的实践经验，希望能与各位学者探讨交流，有朝一日实现更加准确、客观的精度评价工具。

---

提示：以下是本篇文章正文内容，欢迎大佬批评指正。

一. EVO精度评估原理：

1. 机器人解算的运动轨迹记为测量值$T_r^b$，$T_r^b$表示机器人相对于本体坐标系的位置和姿态。
2. 高精度测量系统（真值系统）记录的机器人轨迹为真值$T_r^g$，$T_r^g$表示机器人相对于真值坐标系的位置和姿态。
3. 要计算机器人的实时定位误差，需要根据实现$T_r^b$和$T_r^g$的坐标系统一。根据坐标系转换原理，可以将机器人定位算法得到的$T_r^b$（如SLAM解算的定位结果）转换到真值坐标系下得到${T^{\prime }}_r^g$：
   $${T^{\prime }}_r^g=s\ast R_b^g\ast T_r^b+t_b^g\text{\hspace{1em}(1)}$$
   然后根据真值系统记录的机器人运动轨迹$T_r^g$和公式{1}得到的 ${T^{\prime }}_r^g$作差，即可得到机器人得到的实时定位误差（注意：这里认为$T_r^g$和${T^{\prime }}_r^g$时间对齐，在EVO源码中是通过判断$T_r^b$和$T_r^g$的时间戳是否小于设定的阈值即为时间对齐）：
   $$e_r^g={\Vert {t^{\prime }}_r^g-t_r^g\Vert }_2\text{\hspace{1em}(2)}$$
   注意：$t_r^g$表示机器人在真值坐标系下的位置，$T_r^g$表示机器人在真值坐标系下的位置和姿态，这里计算定位误差只考虑位置。
   

图1. EVO计算的实时定位误差

二. EVO的核心和本质

EVO的核心：就是根据一系列的$t_r^b$和$t_r^g$,首先筛选出时间对齐的$t_r^b$和$t_r^g$点对，然后根据Umeyama原理计算本体坐标系到真值坐标系的位置$t_b^g$，姿态$R_b^g$和尺度因子$s$。其中，EVO中的s默认为1，一般在单目SLAM中才会通过-s的指令计算尺度因子$s$。这其实与坐标系转换原理的七参数布尔莎法，罗德里格矩阵的坐标系转换方法等原理相同。（个人测试发现利用罗德里格矩阵的坐标系转换方法与Umeyama原理得到的$t_b^g$和$R_b^g$相同）。
注意：
（1）EVO的本质就是求解本体坐标系到真值坐标系的位置$t_b^g$，姿态$R_b^g$和尺度因子$s$；
（2）EVO估计$t_b^g$和$R_b^g$是利用了整个轨迹上时间对齐的$t_r^b$和$t_r^g$点对；
（3）这种计算方法其实有利有弊。优点在于利用整个轨迹的点对计算$t_b^g$，$R_b^g$和$s$时，观测量非常丰富，计算的结果更加准确；缺点也十分显著，其不能真实反映系统实际的实时定位误差。图1所示为evo计算得到的实时定位误差。但是实际上，系统从0时刻开始，实时定位误差应该为0，然后逐渐累积，这与实际情况并不十分符合。

三. EVO的显著缺陷

上文提到了EVO的显著缺点，其实很多学者并没有十分关注或者关注了但是没有想办法解决问题。因为，至少目前来看EVO仍是SLAM中精度评估最主流的工具之一，笔者其实很早就发现了这个问题，也一直想要去解决这个显著的缺陷，之前也写过一些复现和改进EVO的代码，但都是偏工程性的做法。至少目前想到的一些改进方法，还难以从理论和数学推导上完胜EVO，所以即使做出改进，也很难让多数学者信服和推广使用。但是，EVO的显著问题的确是实时客观存在的。
此外，EVO在计算RPE时候还会涉及到真值系统和本体坐标系统的姿态对齐问题，这会导致EVO计算的RPE误差增大，其解决方法类似于通过手眼标定原理实现真值系和本体系的重合。RPE的具体问题和改进方法日后会进一步更新补充。
因此，笔者日后非常想要做的一件事，是制作并开源一套更加客观、准确的SLAM精度评估工具。也欢迎有此想法的同学交流探讨。

四. SLAM精度评估的其他开源工具

1. viral_eval

新加坡南阳理工大学谢立华教授团队开源的一款基于Matlab实现的SLAM精度评估工具箱。画图还是比较漂亮的，在Windows/Ubuntu下用起来都比较方便，但是了解的人似乎比较少，可能他们之前很多工作都是基于UWB的多源传感器导航和SLAM。（下图展示几幅viral_eval工具的画图效果）
viral_eval

2. SLAM_Evaluation

上海交通大学邹丹平教授团队开源的EVO改进版SLAM_Evaluation，主要解决计算RPE时真值系和本体系不重合导致的计算误差较大的问题（本质上可以通过手眼标定的方法解决，但实际中会涉及到时间同步等比较复杂的工程问题）。
SLAM_Evaluation

图3. EVO评估时出现的错位问题

3. 其他开源工具

后续会继续补充完善，敬请期待。

---

总结

EVO是SLAM中常用的精度评价工具之一，但其缺陷也十分显著。希望有朝一日能够制作并开源更加准确、客观的SLAM精度评价工具。欢迎各位大佬交流探讨。

参考文献

1. Sturm J, Engelhard N, Endres F, et al. A benchmark for the evaluation of RGB-D SLAM systems[C]//2012 IEEE/RSJ international conference on intelligent robots and systems. IEEE, 2012: 573-580.
2. https://github.com/ntu-aris/viral_eval
3. https://github.com/SJTU-ViSYS/SLAM_Evaluation