探秘SLAM之ORB-SLAM

1.背景

  ORB-SLAM是西班牙Zaragoza大学的Raul Mur-Artal编写的视觉SLAM系统。第一个版本主要用于单目SLAM，而第二个版本支持单目、双目和RGBD三种接口。ORB-SLAM是一个完整的SLAM系统，包括视觉里程计、跟踪、回环检测，是一个学习slam很好的开源项目。

2.流程

  ORB-SLAM整体流程如下图所示：

  从图中可以看到ORB-SLAM可以分为3个线程：

• 跟踪（Tracking）
• 局部地图构建（LocalMapping）
• 闭环检测（LoopClosing）

2.1 跟踪（Tracking）

  Tracking主要工作是确定每帧的位姿和确定关键帧。可以分成以下几个部分：

• 提取ORB特征
• 初始化（单目）
• 根据上一帧或者重定位来估计初始位姿
• 局部地图跟踪
• 确定关键帧

2.1.1提取ORB特征

  特征点由关键点（Key-point）和描述子（Descriptor）两部分组成。提取ORB特征可以分成以下几个步骤：

• FAST关键点提取
• BRIEF描述子计算

2.1.2初始化（单目）

  在单目模式下要进行初始化。可以分为以下几个过程：

（1） 通过匹配选取两个可以作为起始两帧的初始帧。选取是两个特征点数目大于100的两个连续帧，并进行匹配。
（2）根据匹配计算两帧之间的位姿。用两个线程同时计算特征点共面和非共面情况下的两个约束H（单应性矩阵）和F（基础矩阵）。可以通过若干次RANSAC抽样计算出最优的H和F矩阵。计算好约束关系后，根据约束关系计算累积误差，根据比值确定选择哪种约束。最后计算出两帧之间的位姿。
（3） 三角化测量初始的特征点云深度，进而获得点云地图。
（4）BA优化初始点云。

2.1.3根据上一帧或者重定位来估计初始位姿

（1）根据上一帧来估计初始位姿。假设相机为恒速运动模型来估计当前帧的相机位姿，并且在当前帧中搜索前一帧观察到的地图点时，可以根据该运动来缩小搜索范围。然后将上一帧跟踪到的地图点投影到当前帧，然后寻找匹配点，如果匹配点过少，则增大搜索范围。寻找到足够匹配点后，利用匹配点优化估计的初始位姿。如果根据运动模型得到的匹配点数目少于20，就会利用关键帧进行匹配。

（2）重定位来估计初始位姿 对每一个候选关键帧，通过Bow进行匹配，如果有足够的匹配（匹配特征数超过15个），则构建PnP进行求解，计算出当前帧的相机外参。最后求出其初始的位姿。

2.1.4局部地图跟踪

（1）对关键帧的更新和局部地图点的更新。
（2）将局部地图点与当前帧进行匹配。遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索。将局部地图点投影到当前帧，判断局部地图点是否在当前帧的视野内，对视野内的地图点通过投影进行特征匹配。
（3）BA优化相机位姿。

2.1.5确定关键帧

• 相邻两个关键帧之间有一定的间隔
• 当前的帧至少匹配到了 50 个 Map Point
• 当前帧匹配到的Map Point的个数不能超过参考关键帧对应Map Point的90%
• 局部地图优化空闲的时候

2.2局部地图构建（LocalMapping）

  LocalMapping的线程主要确定更多的约束，对关键帧的位姿和map point点的位置进行修正。可以分成以下几个部分：

• 添加关键帧
• Map Point的剔除
• 新的Map Point的创建
• 局部BA优化
• 局部关键帧的剔除

2.2.1 添加关键帧

  每添加一个keyframe，维护Covisibility Graph，Spanning Tree，Map以及计算该帧的词袋表示确定匹配，为三角化做准备。

2.2.2Map Point的剔除

  检查最近加入的这些地图点，并将一些冗余的地图点从最近地图点的列表中去除 。剔除规则如下：

• 地图点是被标记为bad。
• 有超过25%的关键帧可以看到这个点。
• 未被超过2个关键帧看到，并且当前关键帧的ID和看到该点的第一个关键帧之间的ID之差大于等于2的时候。
• 当前关键帧的ID和看到该点的第一个关键帧之间的ID之差大于等于3的时候。

2.2.3新的Map Point的创建

  重新当前关键帧附近的关键帧与当前关键帧之间的对极约束，通过该对极约束还原当前关键帧到附近关键帧的变换，并通过这种变换进行三角化计算。接着分别检查新得到的点在两个平面上的重投影误差，如果大于一定的值，直接抛弃该点。最后检查尺度一致性。

2.2.4局部BA优化

  添加完了关键帧及对应map point，有了这些新的约束，进行一次局部BA优化。

（1）确定局部关键帧。
（2）确定局部的map point。
（3）找到可以观察到这些局部地图点但非局部关键帧的关键帧作为固定关键帧，设置为fixed，不进行优化。
（4）局部map point中的map point与观察到该map point的所有关键帧构建边。
（5）去除一些不好的边，再次进行优化。

2.2.5局部关键帧的剔除

  剔除冗余的关键帧。冗余的关键帧为该帧上对应的map point能被其它至少3个关键帧观测到90%以上。

2.3闭环检测（LoopClosing）

  Local Mapping主要消除累计误差，解决尺度漂移问题。可以分成以下几个部分：

• 闭环条件检测
• 计算相似变换
• 闭环融合
• Essential Graph优化

2.3.1闭环条件检测

（1） 确定局部关键帧。
（2） 计算当前关键帧的词袋(BOW)与所有局部关键帧的词袋(BOW)的相似度，得到最小相似度。
（3） 在关键帧数据库中找到相似度不小于最小相似度的关键帧作为闭环的候选关键帧。
（4） 对每一个候选关键帧检测其与之前回环的一致性。

2.3.2计算相似变换

（1）计算当前关键帧与可能的候选闭环帧的匹配。
（2）用RANSAC算法计算Sim3，并优化所有的对应。
（3）两帧相互投影，根据对应的map point寻找对应帧的匹配。
（4）优化重投影误差。
（5）将闭环帧及其相邻帧对应的map point投影到当前帧上，寻找更多的匹配。
（6）如果匹配数大于40，则认为计算成功。

2.3.3闭环融合

  将当前关键帧对应的map point变换到闭环帧对应的map point，重新构建covisibility graph。当前帧的位姿会根据相似变换而被矫正，同时所有与其相连的关键帧也会被矫正。

2.3.4Essential Graph优化

  在 Essential graph上进行姿态图优化。这样就可以将闭环的误差分散到整个图中。