ORB-SLAM2算法单目流程

ORB-SLAM系列算法框架比较复杂，下面来梳理一下单目算法的流程。

一：单目初始化

单目初始化也就是单目的地图初始化。流程如下：

* Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧

* Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧

* Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对

这里单目初始化阶段寻找两帧之间的特征点匹配对时采用“划圆划分方格”的方式进行。

* Step 4：如果初始化的两帧之间的匹配点太少，重新初始化

* Step 5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints

估计H矩阵和F矩阵是通过从匹配特征点对中随机选择了8对匹配特征点为一组，一共8组。这里采用了多线程计算。计算得分来选择是使用H矩阵还是F矩阵，平面倾向于单应矩阵H，非平面偏向于基础矩阵F。

分解H或F矩阵求解会得到4种R和t的组合，选出最佳组合的方法是：若某一组合使恢复得到的3D点位于相机正前方的数量最多，那么该组合就是最佳组合。

这里得到的两帧间的相对运动t是一个单位向量，并没有确定整个SLAM过程的尺度。

* Step 6：删除那些无法进行三角化的匹配点

* Step 7：将三角化得到的3D点包装成MapPoints

这里的3D点在分解H或F步骤就已经三角化计算出。

二：跟踪线程（每一帧）

ORB-SLAM2中的跟踪分为三种形式，分别是：参考关键帧跟踪、恒速模型跟踪、重定位跟踪、局部地图跟踪。

其中参考关键帧跟踪、恒速模型跟踪、重定位跟踪叫做第一阶段跟踪（初步的跟踪）。

参考关键帧跟踪

场景：地图刚初始化成功后、恒速模型跟踪失败后。

跟踪方式：通过词袋进行特征匹配，通过最小化重投影误差的方式进行求解位姿。

这里参考关键帧分为两种：在这里是距离当前帧最近的关键帧作为参考关键帧。
另外一种是在跟踪局部地图中：与当前帧共视程度最高的一级共视关键帧。

恒速模型跟踪

场景：地图初始化成功后的正常场景下都是恒速模型跟踪。

跟踪方式：通过参考关键帧更新上一帧位姿（关键帧位姿会更新），根据之前估计的速度得到当前帧初始位姿，用上一帧的地图点进行投影跟踪（化圆），如果匹配点不够，则扩大半径搜索。

这里双目和RGBD相机会生成临时地图点的操作。

上一帧是普通帧，单目中普通帧和关键帧中都不会进行三角化操作产生地图点，所以上一帧的地图点的来源主要是：

1.通过恒速跟踪模型从最新的关键帧传递而来的。

2.在局部地图跟踪环节，从上一帧的局部关键帧的局部地图点投影而来。

而单目相机中关键帧中的地图点是通过局部建图线程产生的，双目和RGBD相机在创建关键帧的时候也会产生地图点。

重定位跟踪

场景：跟踪失败的情况下。

跟踪方式：先通过词袋进行快速匹配，在关键帧数据库中寻找相似的候选关键帧，再EPNP算法求解一个相对准确的初始位姿，之后再反复进行投影匹配和BA优化位姿。

局部地图跟踪

局部地图跟踪属于第二阶段跟踪，目的是使跟踪的定位更加准确。

场景：第一阶段跟踪后进行。

跟踪方式：将当前帧的局部关键帧对应的局部地图点投影到该帧中（地图点投影，跟恒速模型跟踪方式一样），得到更多的特征点匹配关系，对第一阶段的位姿再次进行优化。

在新增关键帧时，会产生一些地图点。

三：局部建图线程（关键帧）

该线程的目的是让已有的关键帧之间产生更多的联系，产生更多可靠的地图点，优化共视关键帧的位姿及其地图点，使得跟踪更加稳定，参与闭环的关键帧位姿更加准确。

* 处理新的关键帧

局部地图中的关键帧来自跟踪线程，这些关键帧会进入一个队列中，等待局部建图线程的处理。

内容包括：处理列表中的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等。

* 剔除不合格的地图点

* 生成新的地图点

在局部建图线程中，会在共视关键帧之间重新进行特征匹配、三角化，生成新的地图点，这对稳定的跟踪非常重要。这是单目模式下除初始化环节外生成地图点的唯一方式。

* 检查融合当前关键帧和相邻帧的地图点

* 关键帧的剔除

在跟踪线程中关键帧的创建是比较频繁的，在这里会进行冗余关键帧的删除，提高局部BA的速度。

* 局部地图BA

进行局部关键帧和局部地图点的BA优化。

顶点：待优化的局部关键帧和局部地图点。

边：局部地图点和观测到它的关键帧的观测关系，为二元边。

四：闭环线程

Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的，在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue。

寻找并验证闭环候选关键帧

这里确定闭环候选关键帧的条件比较复杂，先寻找与当前关键帧具有共同单词但不共视的关键帧（通过词袋匹配）叫做“候选关键帧”。然后去才找到这些候选关键帧的前10个共视关键帧，构成“组”。取得分超过一定阈值的组叫做“子候选组”。子候选组中的得分最高的关键帧作为“闭环候选关键帧”。

闭环线程中每次闭环检测都会产生一些子候选组，如果这次闭环检测中的子候选组中和上次闭环检测中的子候选组中存在同一个关键帧，那么这个子候选组的“连续性”+1。第一个连续长度满足要求的子候选组的候选闭环关键帧用于后续的闭环矫正。