Visual SLAM算法笔记

MulinB按：最近在学习SLAM算法，这里作为阅读笔记记录和总结一下。这里关注的主要是基于视觉的Visual SLAM或Visual Odometry，也包括一部分图像和IMU融合的Visual-Inertial Odometry相关算法。

注：下文中部分链接指向Google Scholar及Youtube，有些用户可能无法访问。

一、概述

Simultaneous Localization and Mapping (SLAM)原本是Robotics领域用来做机器人定位的，最早的SLAM算法其实是没有用视觉camera的（Robotics领域一般用Laser Range Finder来做SLAM）。本文主要关注基于camera图像做SLAM的算法，即Visual SLAM算法。本文对SLAM和Visual SLAM不做区分。

其实SLAM是一个研究了30年的topic了，然而到目前仍然没有广泛走进人们的生活。个人认为，其中一个原因是SLAM对实时性要求比较高，而要做到比较精确、稳定、可靠、适合多种场景的方案一般计算量相对较大，目前移动式设备的计算能力还不足够支撑这么大的计算量，为了达到实时性能，往往需要在精确度和稳定性上做些牺牲。因此在具体的应用中，往往需要根据移动设备所具有的传感器组合、计算能力、用户场景等，选择和深度定制合适的SLAM算法。比如，无人驾驶汽车和手机端AR类应用的SLAM算法就非常不同。无人驾驶汽车中，传感器的使用可以更多样化，其中在建地图时和实时定位时的算法又很不一样，需要根据具体情况下使用的传感器类型和可以获得的地图辅助数据类型等信息进行综合性的方案设计，以达到精度和稳定性最大化。而在手机端的AR类应用中，如何利用标配的单目+MEMS IMU在旗舰类手机上得到更精确、更稳定的算法是关键任务。本文主要focus在后者。

关于SLAM的基础入门，这里不多啰嗦，列一些不错的学习资源如下：

• Tomasz Malisiewicz的博客有一篇不错关于SLAM的文章，其中很多link的资源都很好

• 国内有个不错的SLAM组织，叫“泡泡机器人”，其中有很多不错的SLAM课程，可以关注其微信公众号查看往期课程。另外该组织的不少成员都有博客，有很多不错的算法详细介绍的文章

另外再列一些比较强的研究SLAM的组织和牛人：

• ICL的Andrew Davison组：老牌SLAM研究组，专注SLAM十几年，很多SLAM领域的大牛都出自这个组，比如KinectFusion和DTAM的作者Richard Newcombe、G2O的作者Hauke Strasdat等

• Oxford的ActiveVision组：PTAM出自这个组，Andrew Davison也毕业自这个组

• Gatech的Frank Dellaert组：GTSAM出自这个组

• TUM的Daniel Cremers组：LSD-SLAM出自这个组

• UZH的Davide Scaramuzza：SVO和OCamCalib出自这个组

• ETH Zurich的ASL组：OKVIS和BRISK的作者Stefan Leutenegger毕业自这个实验室

• Monash Univ的Tom Drummond：他曾经在Cambridge做过教授，FAST detector的作者Edward Rosten是他的学生，PTAM的作者Georg Klein也是他的学生

• Juan D. Tardos：ORB-SLAM出自他的学生

• UMN MARS Lab的Stergios Roumeliotis：MSCKF 1.0出自当时还在这个实验室读书的Anastasios Mourikis，另外一篇关于quaternion algebra的非常棒的tutorial也出自这个实验室

• Anastasios Mourikis：MSCKF 1.0出自他之手，MSCKF 2.0出自他的学生Mingyang Li

---

二、Monocular SLAM算法笔记

Visual SLAM的两篇开山之作应该是Davison ICCV 2003和Nister CVPR 2004 Visual Odometry的两篇paper了。Andrew Davison是从Robotics领域的视角出发，将经典的non-camera的SLAM算法应用到使用single camera做SLAM的方案中，使用了经典的EKF框架，经过后续改进就是TPAMI 2007的MonoSLAM[2]算法。David Nister是Computer Vision领域3D Vision的大牛（著名的五点法就是他的作品），他的CVPR 2004 Visual Odometry paper是从3D Vision的视角出发设计的一个SLAM方案，整个方案设计的比较简单，只是着重连续较少帧的相对camera pose的估计，并没有完整的地图维护机制，因此容易造成drift。不过其3D Vision的算法和思想是后续很多的SLAM算法中都使用到的，这里先从Nister的这篇VO paper开始介绍。

Nister VO (2004) [1]

下面是Nister这篇VO paper的算法流程：

Nister的这篇paper重点介绍了vision相关的算法及实现细节（包括如何高效实现角点提取和match等），但在系统流程上有些介绍的并不详细，比如上面InnerLoop和OuterLoop的次数和条件等。算法中的很多思想在后续的Visual SLAM算法中都有沿用，比如2D to 2D用五点法计算初始pose、3D to 2D用P3P计算pose、2D correspondence -> essential matrix -> pose -> triangulation -> 3D to 2D correspondence -> P3P -> optimization的流程及子模块的迭代方式等（并不是说所有这些3D Vision算法都是Nister首创，重要的是整个流程在Visual SLAM中的应用）。

• 算法优点：没有对motion model的假设，主要靠三点法P3P来计算相机pose；在计算过程中引入多次bundle adjustment提高精度
• 算法缺点：算法流程具体细节描述的不太清晰；没有比较fancy的map points维护机制；插入firewall阻止误差累积的方式比较原始，无法消除drift；方法比较简单，没有跟踪丢失后重定位、回环检测等现代SLAM方法中比较常见的机制

MonoSLAM (2003-2007) [2] (code available)

Andrew Davison的MonoSLAM是将传统机器人领域中基于laser range-finder的EKF-SLAM应用到了single camera的SLAM中，算法相对于Nister VO更加完整，其中关键的提高在于通过guided feature matching取代invariant feature matching提高了计算速度。这里有个介绍MonoSLAM的不错的slides，在Github上有MonoSLAM的代码。另外，更入门的材料，关于Kalman Filter和Particle Filter的介绍，最容易intuitively理解的是udacity上Sebastian Thrun的视频课程。另外，这里有一篇非常不错的对Kalman Filter介绍的blog。

具体点讲，在MonoSLAM中，每一个state由一个当前状态的最佳估计state vector $\hat{\mathbf{x}}$，以及一个表示该状态uncertainty的协方差矩阵 $\mathrm{P}$ 表示。其state vector包括相机的pose、velocity、angular velocity、以及场景中所有3D map points的坐标。该模型是一个对于state vector中参数的single modal multivariate Gaussian modeling，借用一个上述slides的图表示：

假设场景中有 $N$ 个3D map points，那么每次更新上述协方差矩阵的计算复杂度就是 $O(N^2)$，为了维持在realtime的performance，通常需要控制3D map points的个数 $N<100$。虽然这表面看起来不是很高效，paper中介绍了原因如下：之所以选择一个full covariance matrix而不选择使用covariance submatrix减小计算，是因为MonoSLAM的目标是在一间room大小的场景中进行repeatable localization，这种情况下相机的view会较多的与彼此重叠，会有频繁的loop closure发生，这种情况下用一个full covariance matrix会更加精确，而且100个左右的map points足够表征一间room大小的场景。

MonoSLAM中对于特征的检测和匹配如下：特征点检测使用的是Shi-Tomasi corner detector (类似于Harris detector，这里有其区别)，特征匹配是靠warped image patch进行NCC匹配（每个patch template保存其所在view的surface orientation，匹配时根据后续帧的pose将该patch template投影过去），paper中特意提到该patch template一旦初始化将不进行更新（为防止drift）。更进一步，patch template的surface orientation也是通过一个单独的Kalman filter进行estimate的(Section 3.8)。

基于Kalman filter的算法在每个时刻的计算一般分为两步：第一步，predict step；第二步，update step。在第一步中一般是根据运动模型或者控制模型预测state vector，是uncertainty propagation的过程，其中用到的运动模型或者控制模型需要根据实际场景设置其uncertainty的参数，直接影响covariance matrix的计算。在第二步中是根据观测到的结果来估计最佳state vector并减小uncertainty。借用wikipedia的公式，Kalman filter的计算过程如下：

作为对比，EKF是将上述predict step中的变换矩阵 $\mathrm{F}$ 更换成非线性的函数 $f(-)$，将update step中的观测矩阵 $\mathrm{H}$ 更换成非线性的函数