三维重建7：Visual SLAM算法笔记
VSLAM研究了几十年，新的东西不是很多，三维重建的VSLAM方法可以用一篇文章总结一下。
此文是一个好的视觉SLAM综述，对视觉SLAM总结比较全面，是SLAM那本书的很好的补充。介绍了基于滤波器的方法、基于前后端的方法、且介绍了几个SensorFusion方法，总结比较全面。并且文中给出了代码的下载链接，比较方便。
原文链接：Visual SLAM算法笔记
摘抄部分，如有不适，请联系删除或者移步原文链接
一、Visual-Inertial Odometry算法笔记
名字缩写太多，我有点凌乱了，做个区分：
DVO: TUM的基于RGBD camera的VO方法
SVO: Gatech的基于semi-direct的hybrid VO方法
DSO: TUM的direct sparse VO方法
X、Sensor Fusion笔记
使用monocular camera + IMU的方案来做SLAM/Odometry，一般被称作Visual-Inertial Odometry (VIO)或者Visual-Inertial Navigation System (VINS)。这一类paper大多出自Robotics社区，主要focus在如何更好的在Visual SLAM中融合IMU数据。IMU数据不单可以帮助resolve单目的scale ambiguity，一般情况下还可以提高SLAM的精度和鲁棒性。需要注意的是，想要从IMU数据获得准确的姿态没那么容易，一般需要做sensor fusion，从经典的complementary filter做gyroscope、accelerometer、magnetometer的融合，再到Mahony filter等更复杂的融合算法，有很多可以选择的算法，其精度和复杂度也各不相同。现在的Android系统里一般可以直接获得手机姿态，至于其中用了哪种融合算法本人还没有仔细研究过，精度也有待考察。在Robotics社区的VIO paper中，一般是直接用原始的IMU数据或者经过简单滤波的数据，一般需要对IMU的bias进行建模（尤其在MEMS IMU中，所谓的零飘和溫飘对精度影响很大，有些要求比较高的情况下甚至需要将其置于恒温状态工作）。
MSCKF (2007-2013) [14,15]
基于Kalman filter的MSCKF跟EKF-based SLAM一样也是出自Robotics社区，从MSCKF 1.0 [14]到MSCKF 2.0 [15]，精度得到了不错的提高，据说Google Project Tango中的SLAM算法就是用的MSCKF算法。
传统的EKF-based SLAM做IMU融合时，跟前面介绍的MonoSLAM类似，一般是每个时刻的state vector保存当前的pose、velocity、以及3D map points坐标等（IMU融合时一般还会加入IMU的bias），然后用IMU做predict step，再用image frame中观测3D map points的观测误差做update step。MSCKF的motivation是，EKF的每次update step是基于3D map points在单帧frame里观测的，如果能基于其在多帧中的观测效果应该会好（有点类似于local bundle adjustment的思想）。所以MSCKF的改进如下： predict step跟EKF一样，但是将update step推迟到某一个3D map point在多个frame中观测之后进行计算，在update之前每接收到一个frame，只是将state vector扩充并加入当前frame的pose estimate。这个思想基本类似于local bundle adjustment（或者sliding window smoothing），在update step时，相当于基于多次观测同时优化pose和3D map point。具体细节可以参考paper[15]。
OKVIS (2013-2014)[16] (code available)
相对应于MSCKF的filter-based SLAM派系，OKVIS是keyframe-based SLAM派系做visual-inertial sensor fusion的代表。从MSCKF的思想基本可以猜出，OKVIS是将image观测和imu观测显式formulate成优化问题，一起去优化求解pose和3D map point。的确如此，OKVIS的优化目标函数包括一个reprojection error term和一个imu integration error term，其中已知的观测数据是每两帧之间的feature matching以及这两帧之间的所有imu采样数据的积分（注意imu采样频率一般高于视频frame rate），待求的是camera pose和3D map point，优化针对的是一个bounded window内的frames（包括最近的几个frames和几个keyframes）。
需要注意的是，在这个optimization problem中，对uncertainty的建模还是蛮复杂的。首先是对imu的gyro和accelerometer的bias都需要建模，并在积分的过程中将uncertainty也积分，所以推导两帧之间的imu integration error时，需要用类似于Kalman filter中predict step里的uncertainty propagation方式去计算covariance。另外，imu的kinematics微分方程也是挺多数学公式，这又涉及到捷联惯性导航(strapdown inertial navigation)中相关的很多知识，推导起来不是很容易。这可以另起一个topic去学习了。
OKVIS使用keyframe的motivation是，由于optimization算法速度的限制，优化不能针对太多frames一起，所以尽量把一些信息量少的frames给marginalization掉，只留下一些keyframes之间的constraints。关于marginalization的机制也挺有趣，具体参见paper[16]。
ETH Zurich的ASL组另外有一篇基于EKF的VIO paper，叫ROVIO [17]，也有code，具体还没细看，听说鲁棒性不错。
IMU Preintegration (2015-2016)[18] (code available in GTSAM 4.0)
从OKVIS的算法思想中可以看出，在优化的目标函数中，两个视频帧之间的多个imu采样数据被积分成一个constraint，这样可以减少求解optimization的次数。然而OKVIS中的imu积分是基于前一个视频帧的estimated pose，这样在进行optimization迭代求解时，当这个estimated pose发生变化时，需要重新进行imu积分。为了加速计算，这自然而然可以想到imu preintegraion的方案，也就是将imu积分得到一个不依赖于前一个视频帧estimated pose的constraint。当然与之而来的还有如何将uncertainty也做类似的propagation（考虑imu的bias建模），以及如何计算在optimization过程中需要的Jacobians。相关的推导和理论在paper [18]中有详细的过程。在OKVIS的代码ImuError.cpp和GTSAM 4.0的代码ManifoldPreintegration.cpp中可以分别看到对应的代码。
[1]. David Nister, Oleg Naroditsky, and James Bergen.Visual Odometry. CVPR 2004.
[2]. Andrew Davison, Ian Reid, Nicholas Molton, and Olivier Stasse.MonoSLAM: Real-time single camera SLAM. TPAMI 2007.
[3]. Ethan Eade and Tom Drummond. Monocular SLAM as a Graph of Coalesced Observations. ICCV 2007.
[4]. Georg Klein and David Murray. Parallel Tracking and Mapping for Small AR Workspaces. ISMAR 2007.
[5]. Georg Klein and David Murray. Improving the Agility of Keyframe-based SLAM. ECCV 2008.
[6]. Georg Klein and David Murray. Parallel Tracking and Mapping on a Camera Phone. ISMAR 2009.
[7]. Hauke Strasdat, J.M.M. Montiel, and Andrew Davison.Visual SLAM: Why Filter?. Image and Vision Computing 2012.
[8]. Raul Mur-Artal, J. M. M. Montiel, and Juan D. Tardos.ORB-SLAM: A Versatile and Accurate Monocular SLAM System. IEEE Transactions on Robotics 2015.
[9]. Richard Newcombe, Steven Lovegrove, and Andrew Davison.DTAM: Dense Tracking and Mapping in Real-Time. ICCV 2011.
[10]. Jakob Engel, Jurgen Sturm, and Daniel Cremers.Semi-Dense Visual Odometry for a Monocular Camera. ICCV 2013.
[11]. Christian Forster, Matia Pizzoli, and Davide Scaramuzza.SVO: Fast Semi-Direct Monocular Visual Odometry. ICRA 2014.
[12]. Jakob Engel, Thomas Schops, and Daniel Cremers.LSD-SLAM: Large-Scale Direct Monocular SLAM. ECCV 2014.
[13]. Jakob Engel, Vladlen Koltun, and Daniel Cremers.Direct Sparse Odometry. In arXiv:1607.02565, 2016.
[14]. Anastasios Mourikis, Stergios Roumeliotis. A multi-state constraint Kalman filter for vision-aided inertial navigation. ICRA 2007.
[15]. Mingyang Li, Anastasios Mourikis. High-Precision, Consistent EKF-based Visual-Inertial Odometry. International Journal of Robotics Research 2013.
[16]. Stefan Leutenegger, Simon Lynen, Michael Bosse, Roland Siegwart, and Paul Timothy Furgale.Keyframe-based visual–inertial odometry using nonlinear optimization. The International Journal of Robotics Research 2014.
[17]. Michael Bloesch, Sammy Omari, Marco Hutter, and Roland Siegwart.Robust Visual Inertial Odometry Using a Direct EKF-Based Approach. IROS 2015.
[18]. Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza.On-Manifold Preintegration for Real-Time Visual-Inertial Odometry. IEEE Transactions on Robotics 2016.
[19]. George Vogiatzis, Carlos Hernandez. Video-based, Real-Time Multi View Stereo. Image and Vision Computing 2011. (Supplementary material)
