田里的小粟读ORBSLAM2

ORB-SLAM2——一种开源的单目、双目、RGBD相机的SLAM系统

• 摘要
• 介绍
• 相关工作
• ORB-SLAM2
• 评估
• 结论

        为了更好的理解ORBSLAM2的代码，先吧ORBSLAM和ORBSLAM2的论文内容理清，也方便以后的学习。

摘要

        我们提出针对单目、双目、RGBD相机的完整的SLAM系统，系统包含了地图重利用、回环检测和重定位模块。这个系统可以在标准CPU上实时运行，适应各种环境，包括手持室内环境、工业环境下的无人机飞行、在城市中行驶的汽车。后端基于单双目的BA，因此具有较精准的轨迹估计。我们的系统包含了一个轻量级的定位模式，它在未建图的区域利用视觉里程计跟踪，然后利用地图点之间的匹配来进行零漂移定位。我们在29个公开的序列中进行测试，证明了我们的系统是最精确、最完整的SLAM系统。我们将代码开源了。

介绍

        SLAM近20年受到包括计算机视觉、机器人研究者以及高科技公司的广泛关注。SLAM对未知环境进行建图同时定位传感器的位置，它十分注重实时性。在众多传感器中，相机价格低廉而且可以提供环境丰富的信息用于进行鲁棒和精准的定位。因此主要传感器为相机的视觉SLAM是研究热点。位置识别是SLAM系统用于重定位和闭环检测的重要模块，以应对由于遮挡或者夸张的动作导致的定位失败或者重新初始化。
        VSLAM可以只用单目相机。但是由于单目相机无法观测深度，因此由它构建的地图尺度未知。而且单目系统需要用多视图或者滤波的方法来进行初始化。再者单目详细容易出现尺度漂移问题且对于纯旋转场景容易失败。通过使用双目或者RGBD相机，这些问题可以被解决而构建更加可靠的SLAM系统。
        这篇文章在已有的danmORBSLAM的基础上提出ORBSLAM2，贡献如下：

• 第一个单目、双目、RGBD的开源SLAM系统，包含了回环检测、重定位和地图重利用
• 我们的RGBD结果显示通过使用BA比ICP或者像素深度最小化的方法精度更高
• 通过联合远双目和单目的数据，我们的双目系统比直接双目系统更加精准
• 一个轻量级的定位模式可以高效地进行地图重利用

        图1显示使用双目或者RGBD数据的ORBSLAM结果的一个示例。


        文章的剩余部分，II部分讲了相关工作、III描述了整个系统、IV部分描述了评估结果、V部分做总结。

相关工作

        这一部分，我们讨论了双目、RGBD SLAM的相关工作。
A 双目 SLAM
        早期双目SLAM的代表作是[5]，基于条件独立分布，EKF-SLAM可以适用于大范围的环境。更重要的是，它是第一个讨论远近点的双目SLAM。它们的工作指出如果地图点的深度低于基线长度的40倍，那么深度可以被准确地估计出来。我们采用了这种策略对远近点采取不同的策略。
        大多数的现代双目SLAM是基于关键帧然后在局部地图使用BA优化的方法。[8]中对内窗关键帧的点使用BA优化，对外窗的姿态使用BA优化。通过限制窗的大小，处理时间可以维持稳定，但是不能保证全局连续性。RSLAM使用路标点和姿态图的相对表示然后进行优化，这可以使处理时间保持稳定。RSLAM可以进行回环但是不能保证全局连续性。S-PTAM使用局部BA但是它缺少大的回环检测。和那些在局部地图对关键帧使用BA的方法类似，这样可以使用处理的复杂度和地图的尺度相互独立，这使得我们的系统可以在大的环境中运行。但是我们的目标是建立全局连续的地图。当进行闭环时，我们系统和RSLAM类似融合两种边以使得跟踪可以使用旧地图继续然后使用BA进行姿态图优化来最小化漂移误差。
        最近的LSD-SLAM是一个半稠密直接法的系统，它通过高梯度最小化光度误差。不依靠于特征，所以这种方法对动作模糊和弱纹理的环境更加鲁棒。但是无法用模型评估的卷帘快门和非伯朗反射率的影响会使得系统性能呢急剧下降。

A RGB-D SLAM
        最早最著名的RGBD-SLAM应该是KinectFusion。这个系统将所有的深度数据融合到一个很大的模型当中然后使用ICP的方法进行跟踪。这个系统只使用于小的场景由于它的复杂表示和缺少回环。Kintinuous通过使用滚动循环缓冲器和在位置识别和姿态图优化中使用回环检测使得系统可以在大的环境中运行。
        最早受到欢迎的开源系统应该是RGBD-SLAM。这是一个基于特征的系统。前端使用帧与帧的特征匹配和ICP方法来进行跟踪；后端使用启发式的回环检测来进行姿态图优化。后端与RGBD-SLAM类似的DVO-SLAM通过最小化光度和深度的共同误差的视觉里程计来优化姿态图。它使用启发式的方法在之前所有帧中寻找回环候选帧，而不是使用位置识别。
        [15]中构建了基于点元的环境地图。它是一种不考虑姿态基于质心的方法在回环时对地图使用非刚体变换。这个系统可以进行细节重建而且定位精度比较高，但是系统由于计算的复杂性只能运行在室内环境中。
        正如[8]中那样，我们的ORB-SLAM使用深度信息来合成和图片中特征提取有关的立体坐标。这种方法适用与立体相机和深度相机。不同与以上所有的工作，我们的后端使用BA优化并且进行全局连续的重建。因此我们的方法是轻量级的可以在标准CPU上工作。我们的目标是可以长期并且全局连续地重建而不是构建稠密地图。但是通过精确的关键帧姿态可以在局部区域构建精细的地图。

ORB-SLAM2

        ORBSLAM2是在ORBSLAM的基础上改进的，主要涉及的地方展示如下：系统概述如图2。系统包含3个主要的线程。1）跟踪线程负责从每一帧中定位相机位置并寻找匹配特征然后使用纯运动BA最小化重投影误差。2）局部地图线程用于构建局部地图并使用局部BA进行优化。3）回环检测检测大回环来减小漂移误差并且使用姿态图优化。这个线程会启动第4个线程在姿态图优化后进行全BA优化，以此来计算最优的结果。

        这个系统对于跟踪、建图和位置识别使用相同的特征。这些特征对于旋转和尺度有很好的鲁棒性，并且对相机的自动增益、自动曝光和光度变化具有很好的不变性。而且它们可以进行快速的提取和匹配并且在位置识别中表现出很好的精度。在下一部分我们将介绍立体和深度信息是怎么加入系统的。

A 单目、近双目点和远双目点
        ORBSLAM2是基于特征法的系统，在预处理部分进行特征提取。接着图片将会被丢弃往后的操作都是对这些特征进行的，因此系统对应双目输入或者深度图输入是类似处理的。我们的系统处理产生单目和双目的关键点并将它们分成近点和远点。
        立体关键点被定义为三维向量x_s=(u_L,v_L,u_R)，其中(u_L,v_L)是左视图的坐标，u_R是右视图的水平坐标。对于双目图像，我们在两张图片中提取ORB特征，然后为左视图的特征在右视图寻找匹配的特征。假设立体图像被矫正过对极线是水平的，这种方法可以被高效地进行。我们通过左右特征点匹配来生成立体关键点。对应RGBD相机，我们在RGB图像中提取ORB特征，然后对于(u_L,v_L)上的特征通过深度值d转成右视图坐标

        其中f_x是水平焦距，b是基线长度。深度传感器的不确定被转化成右视图的不确定性。通过这样的方式立体相机的特征和RGBD的特征对于接下来的处理都是等价的。
        如果立体关键点的深度小于基线的40倍那么就是近点否则是远点。近点通过三角化可以精确地估计尺度、平移矩阵和旋转矩阵。另一方面远点可以提供精确的旋转信息但是尺度和平移信息可能不准确。如果远点被多个视图观察到时我们三角化这些点。
        单目关键点定义为左视图的两个坐标(u_L,v_L)，这适用于立体匹配失效和RGBD有一个无效的深度值的情况。这些地图点只有当被多个视图看到时才进行三角化，可以提供尺度信息，但是不提供旋转和平移信息。

B 系统启动
        使用立体相机或者RGBD相机的好处是仅凭一帧就可以知道深度信息而不需要通过SFM的方法。在系统启动时我们将第一帧当做关键帧，将它的姿态作为初始姿态，并使用立体关键点构建初始化地图。

C 利用单目和立体相机的约束来进行BA
        我们的系统在跟踪线程使用纯运动BA来优化姿态图，在局部地图线程优化局部窗的关键帧和地图点，在回环检测模块使用全BA优化所有关键帧和地图点。
        纯运动BA优化相机方向R（属于SO(3)）以及位置t(属于R³)，最小化匹配的3D地图点Xⁱ和关键点xⁱ_(.)的重投影误差，其中单目为xⁱ_m(R²)，立体为xⁱ_s(R³)，其中i是匹配点的序列值。

        其中$\rho$ρ表示鲁棒代价函数，$\Sigma$Σ表示关键点尺度的协方差矩阵。投影函数定义如下：

        其中（f_x,f_y）是焦距，（c_x,c_y）是关键点，b是基线距离。

        局部BA优化共视关键帧K_L以及关键帧看到的所有地图点P_L。其他观测到地图点P_L的关键帧K_F参与代价函数但是优化时固定。定义X_k是第k帧中属于点P_L的关键点。优化项如下：

        全BA是是局部BA的特例，它对除了第一帧外的所有关键帧和地图点进行优化。

D 闭环和全BA
        回环检测分为两步：第一步检测出回环并确定有效；第二步使用回环进行姿态图优化。作为对比的可以出现尺度漂移的单目ORB-SLAM，立体或者深度信息会让尺度可以观测，有效性验证和姿态图优化不在要求处理尺度漂移问题，尺度漂移将基于刚体转换而不是相似性变换。
在ORB-SLAM2中我们对姿态图使用全局BA来得到最优结果。这个优化可能非常消耗计算资源因此我们将它放在单独的线程中，这让系统可以继续创建地图和检测回环。然而当使用当前状态的BA输出融合时会带来挑战。当正在优化过程中我们检测到了闭环，我们将停止优化而重新使用全BA进行优化。当全BA结束时，我们需要用在优化进行时插入的未更新的关键帧和地图点来融合更新被全BA优化的关键帧和地图点。这个可以通过生成树整合已经更新和未更新的关键帧之间的联系。根据参考关键帧的矫正，未更新的地图点将被调整。

E 关键帧插入
        ORBSLAM2采用ORBSLAM的策略来插入关键帧和剔除关键帧。远近立体地图点的差别让我们可以对关键帧插入时添加新的条件。这对于大部分场景都距离相机比较远的场景（如图3所示）是非常有帮助的。在这样的环境中我们需要足够多的近点来精确估计平移，因此如果跟踪的地图点少于$\tau$τ_t,那么这一帧可以至少创建$\tau$τ_c个近的立体点，系统将插入新的关键帧。实验发现$\tau$τ_t=100,$\tau$τ_c=70时系统效果比较好。

F 定位模式
        我们整合了适用于对已经建好的地图或者地图没有太大变化场景进行长期定位的定位模式。在这个模式中局部建图和回环检测线程将被终止，定位只依靠跟踪线程并在必要的时候使用重定位。在这个模式中权衡了视觉里程计的匹配和地图点的匹配。视觉里程计的匹配是从当前帧提取的ORB特征和由立体或深度信息创建的3D点之间的匹配。这些匹配让系统对未建图的定位更加精准。地图点匹配确保定位对已经建好的地图没有漂移。

评估

        我们在3个公开的数据集和当前最好的算法进行对比。我们的ORB-SLAM2运行在i7-4790,16g内存的硬件条件下。我们运行了5次并取中值。

A KITTI 数据集
        KITTI数据集包含了在都市高速路上的立体序列。立体传感器有54cm长的基线，工作频率为10Hz，分辨率为1240*376。序列00,02,05,06,07和09包含回环。我们的系统除了在只有少数帧出现回环的序列09，在其他序列中都可以检测出回环。我们在11个序列上进行了训练，对比LSD-SLAM。我们使用多种评价标准绝对平移RMSE t_abs、平均相对平移t_rel,平均相对旋转r_rel。我们的系统在绝大多数序列中好于LSD-SLAM，一般相对误差低于1%。高速路上01序列表现稍差，在这个序列中因为很少有近点、高速率、低帧率使得很难对平移进行估计。但是旋转是可以被精准地估计出来。

        和单目的ORBSLAM进行比较。单目系统处理序列01时失败，而立体系统成功。在高速序列中，如图3所示，当近点很少时立体系统可以通过立体帧创建地图点而不是像单目系统只能延时初始化。而且立体系统可以估计尺度不会有尺度漂移的问题。

B EuRoc 数据集
        EuRoc包含在大的工业场景中两个房间用无人机拍摄的21个序列。立体传感器有11cm的基线并且提供20Hz的WVGA图像。这些序列按照无人机速度、光照和场景纹理被分成简单、中等和困难序列。在所有序列中都存在回环，ORBSLAM2可以检测出回环并进行地图重利用。表II展示了与LSD-SLAM对比的绝对平移RMSE。ORBSLAM2实现了几厘米的定位精度优于LSD-SLAM。我们的跟踪在V2_03_difficult中由于剧烈抖动而失败,这个序列可以使用IMU进行处理。图6展示和真值的对比。

C TUM RGB-D 数据集
        TUM RGB-D数据集包含了物体重建、SLAM或里程计在不同的纹理、光照和结构条件下的序列。我们使用了绝大多数RGB-D方法都有使用的序列。表III和ElasticFusion、Kintinuous、DVO-SLAM、RGB-D SLAM进行了对比。我们的方法是唯一基于BA的并且在多数序列中表现更好。RGB-D SLAM对序列freiburg2有4%的尺度偏差，可能是由于错误校准导致的。图7展示通过对关键帧的深度图进行后处理得到的点云图。

耗时分析
        为了完整评估我们的系统，我们使用3个不同分辨率和传感器的序列进行耗时分析。每个线程的平均和两个标准偏差展示如下。因为这些序列仅包含一次回环，全局BA和回环检测的一些任务只被执行了一次。平均的跟踪时间低于采样间隔，我们的系统可以实时工作。由于提取立体图像的ORB特征可以并行进行，1000个立体WVGA图像的ORB特征提取和同样数量的单VGA格式图像特征提取消耗的时间差不多。

        回环关键帧的数量和回环所消耗的时间关系展示如上。KITTI在回环时包含更多的关键帧，室内fr3_office序列更加密集，因此这两者回环融合、姿态图优化消耗的时间也更多。更稠密的共视图让局部地图包含更多的关键帧和地图点，因此局部地图跟踪和局部BA也更加消耗计算资源。

结论

        我们提出了适用于单目、立体和RGB-D相机、可以进行重定位、回环检测和地图重利用的、在CPU上可以实时运行的SLAM系统。我们致力于构建全局连续统一的、可靠的、可以进行长期定位的、适用于大范围环境的定位。提出的定位模式，具有重定位功能，系统非常鲁棒、零漂移，轻量级定位。这个模式对于如已经构建好地图的虚拟现实的定位非常有用。
        与最好的算法相比较后发现，ORB-SLAM2有更高的精度。在KITTI数据集中ORBSLAM2是最好的立体SLAM方法。对比与立体视觉里程计方法，ORBSLAM2在已经建好图的环境中可以实现零漂移的定位。
        令人意外的是，我们的RGB-D结果显示，如果要求更加精准的相机定位，BA方法比直接法和ICP的方法更好，计算量更小。
        代码已经开源，是目前唯一的包含单目、双目、RGB-D的SLAM系统。使用单目进行AR应用的例子也在其中进行展示。
        未来的工作可能包括不重叠的多视图场景、支持鱼眼相机或者360度相机，大范围的深度融合、增加对剧烈运动的鲁棒性。