认真的虎ORBSLAM2源码解读(一)：orbslam菜鸟综述

0.前言

注释代码已公开，欢迎交流～～
注释代码已公开，欢迎交流
其他系列文章地址

1. 综述

ORB系统总共有3个线程分别为Tracking，LocalMapping，LoopClosing。
其中Tracking运行在主线程，由Tracking::GrabImageMonocular()进入。
LocalMapping线程由LocalMapping::Run()进入；
LoopClosing线程由LoopClosing::Run()进入；

我将由这个3个线程讲述ORBslam2的单目模式是如何运作的。

2.Tracking跟踪

系统将图片交给Tracking::GrabImageMonocular()后，先将图片转化为灰度图，然后使用图片构建了一个Frame。
注意系统在初始化的时候使用了不同的ORBextractor来构建Frame，是因为在初始化阶段的帧需要跟多的特征点。

然后将进入了Track()，Tracking的大部分功能将在这里实现：
Tracking执行了4个任务：

1. 单目初始化；
2. 通过上一帧获得初始位姿估计或者重定位。也就是求出当前帧在世界坐标系下的位姿$T_2$；
3. 跟踪局部地图（TrackLocalMap()） 求得位姿估计$T_3$。这个步骤回答当前帧的特征点和map中的哪些mappoint匹配。
4. 判断是否需要给localmapping插入关键帧；

2.1. 单目初始化

• 单目初始化需要两帧。具体如何选择这两帧不在这里详述。
• 得到可以用于初始化的两帧后，我们先匹配他们两之间的特征点。
• 根据得到的两帧图像的特征匹配，我们就可以计算单应矩阵H，和基础矩阵F。这里就是高翔的slam十四讲第七章的2D-2D对级约束问题。
• 计算单应矩阵H，和基础矩阵F的过程采用了RANSAC算法来减少误匹配对计算结果的影响。
• 最后计算出的单应矩阵H，和基础矩阵F会有对应的得分，我们根据得分情况来选择使用单应矩阵H，还是基础矩阵F来进行初始化。
• 确定好使用单应矩阵H，还是基础矩阵F之后，会将H或者F分解为R，t。
• 在通过H或者F分解为R，t时，会有多个R，t的组合，我们需要将匹配的特征点通过三角化重投影后，更具投影结果来选择具体是哪个R，t的组合。
• 所以算出R，t后，我们自然也通过三角化得到了通过三角化重投影成功的匹配特征点的相对第一帧的3d坐标。至于剩下那些不能通过R，t重投影成功的匹配点，我们在后面将它剔除，
• 将初始化第一帧的光心作为世界坐标系原点，那么初始化第二帧在世界坐标系中的坐标就可以根据R，t计算出来了。
• 最后将初始化第一帧和第二帧转化为关键帧keyframe，将三角化重投影成功的匹配特征点的3d坐标包裹为mappoint，全部添加到map中。
• 在创建keyframe过程中，我们会记录它的特征点和哪些mappoint匹配。
• 在创建mappoint过程中，我们会记录它能被哪些keyframe的哪些特征点匹配。

2.2. 通过上一帧获得初始位姿估计或者重定位

初始化成功后，后面的帧所面对的情况是一样的。所以我们不从第3帧开始，我们从第i开始。

假设系统给tracking传过来了第i个Frame，我先来阐述下我们现在有什么。我们在map中有许多关键帧keyframe，以及mappoint。
那么来自灵魂的拷问来了，1. 请问你如何快速求得当前帧Frame的相对世界坐标系的位姿呢？

好的，陈独秀同学已经举手示意了，他说可以像初始化那样，通过2D-2D的方式计算当前帧和上一个帧的F和H，进而得到它们之间的位姿关系。好的陈独秀同学，您可以坐下了。想法虽好，但是不可行，因为这样太耗时间了。

其实还有一种方式是通过将当前帧的特征点与map中的mappoint进行匹配，进而得到当前帧在世界坐标系中的位姿。这样这个问题就变成了高翔的slam十四讲第七章的3D-2D的PnP问题。

2.那如何将手中已经当前帧的特征点和map中的mappoint进行匹配呢？
一个一个暴力匹配？不可能的，这辈子都是不可能滴，信不信我tracking分分钟卡死给你看！

orbslam2提供了3种方式回到上述我提的问题，分别为TrackWithMotionModel()，TrackReferenceKeyFrame()，Relocalization()。

2.2.1.通过速度模型获得初始位姿估计

具体如何实施呢？当上一帧跟踪正常的时候，我们会生成一个速度模型mVelocity，使得mVelocity不为空，于是就我们就会进入TrackWithMotionModel()。

速度模型mVelocity就是上一帧和上上帧的位姿变化。由于在短时间内，速度模型不会变化太大，这样我们可以认为上一帧的速度模型（即上一帧和上上帧的位姿变化）等于当前帧的速度模型（即当前帧和上帧的位姿变化）。根据上一帧的求得的速度模型，以及上一帧在世界坐标系的位姿得到当前帧在世界坐标系中的初始位姿$T_0$。

我们将上一帧匹配的mappoint，通过$T_0$投影到当前帧上，然后在投影位置附近搜索当前帧的特征点以加速上一帧mappoint和当前帧的特征点匹配。也就是说，我们以上一帧为跳板，将当前帧的特征点与空间中mappoint进行匹配。

有了当前帧的初始位姿，以及当前帧的特征点与空间中mappoint进行匹配关系。我们就可以在通过bundle adjustment方法优化得到当前帧的较为精确的位姿$T_1$.

得到当前帧较为精确的位姿后，当前帧的有些特征点和mappoint匹配通过这个$T_1$将不成立，形成误匹配于是将它标记为outliner后取消它们之间的匹配。

2.2.2.通过参考关键帧获得初始位姿估计

当速度模型mVelocity为空时，会进入TrackReferenceKeyFrame()通过参考关键帧获得初始位姿估计。

首先，我们通过和tracking的参考关键帧用bow进行特征匹配，进而匹配点参考关键帧的mappoint。
那么参考关键帧怎么得来的呢？如果上一帧被用来转化为了关键帧，那么参考关键帧就为上一帧转化的关键帧。
如果上一帧没有被转化为参考关键帧，那么参考关键帧就为在后面TrackLocalMap()中找到的，和上一帧共视程度最高的那个关键帧；

然后，我们再进行BA位姿优化。为了加速BA，我们将当前帧在世界坐标系中的位姿的初始值$T_0$设置为以上一帧的位姿在世界坐标系中的位姿；

BA位姿优化后，得到当前帧在世界坐标系中的位姿$T_1$。当前帧的特征点和mappoint的匹配有些会变为误匹配，将它剔除。

2.2.2. 重定位

当mState=LOST时，我们会进入Relocalization()。当速度模型不能用，参考关键帧也没有的时候，就意味着tracking已经跟丢了。
这时候，我们先将当前帧转化为BOW，然后将代表当前帧的BOW交给KeyFrameDatabase，它就会返回一个和当前帧很相似的集合作为候选帧。

接着，我们对当前帧和候选关键帧进行通过BOW加速的特征点匹配。并剔除特征匹配数少于15的候选帧。

对于剩下的候选关键帧我们通过epnp结合RANSAC算法进行筛选，并算出当前帧在世界坐标系中的位姿$T_2$。

2.3. 局部地图跟踪

此时，我们通过TrackWithMotionModel()，或者TrackReferenceKeyFrame()或者TrackReferenceKeyFrame()得到了当前帧在世界坐标系中的位姿$T_2$，以及当前帧的特征点和一些mappoint的匹配。

那么我不禁想问，当前帧的特征点和当前我们在map中存储的mappoint的匹配就只有这些了吗？
当然不是，我们还需要再找更多的当前帧的特征点与mappoint的匹配。最直接的方法当然是将map中所有还未匹配的mappoint一个个和当前帧还为匹配的特征点进行匹配啦，不过这样做效率太低啦。最直观的想法，当然是缩小mappoint的搜索方法啦，于是UpdateLocalMap()局部地图应运而生啦！通过这函数，我们缩小了搜索mappoint的范围。

然后在刚才UpdateLocalMap()得到的mappoint范围内，通过位姿投影的方法在SearchLocalPoints()和当前帧的特征点进行匹配。于是我们就得到了更多的当前帧的特征点与mappoint的匹配啦。

因为有更多的mappoint匹配的加入，于是我们就需要进行一次位姿BA，将这些新加入的mappoint匹配的影响也考虑进去，于是就得到了新的当前帧的位姿$T_3$。

记住，每次位姿BA后都会有一些本来匹配的特征点与mappoint的匹配变得不匹配了。剔除这些不匹配的点对后，我们就掰起我们的小手指数一数当前帧还能和多少mappoint匹配，并根据这个数量判断当前帧的这个跟踪是否成功。

2.4. 判断是否需要给localmapping插入关键帧

给localmapping插入关键帧，必须满足以下所有条件：

• 距离上一次全局重定位(global relocalisation)，必须大于20帧；
• LocalMapping线程空闲，也就是说发给LocalMapping的关键帧它都处理完了。或者自从上一次交给LocalMapping线程关键帧已经过了20帧了；
• 当前帧至少跟踪了50个mappoint，也就是说当前帧跟踪要好；
• 当前帧跟踪到的参考帧的mappoint必须少于90%；

3. LocalMapping局部建图

LocalMapping线程由LocalMapping::Run()进入；
它主要做了以下几件事：

• 向map中插入关键帧；
• 新近插入的mappoint的剔除；
• 新建mappoint；
• 局部捆集调整（BA）；
• 剔除冗余关键帧；

3.1. 向map中插入关键帧

• 计算了插入的关键帧的mBowVec，mFeatVec；
• 遍历和关键帧匹配的mappoint
  • 让mappoint知道自己可以被哪些keyframe看到；
  • 更新此mappoint参考帧光心到mappoint平均观测方向以及观测距离范围
  • 在此mappoint能被看到的特征点中找出最能代表此mappoint的描述子
• 更新共视图Covisibility graph,spanningtree；
• 将该关键帧插入到地图map中；

3.2. 新近添加的mappoint的剔除

新近添加入map的mappoint必须经它被创建之后的3帧的检验，以保证它是可追踪以及由于错误的匹配导致的被错误的三角化。因此mappoint必须满足以下两个条件：

• 跟踪到该MapPoint的Frame数相比预计可观测到该MapPoint的Frame数的比例需大于25%；
• 从该点建立开始，到现在已经过了不小于2个关键帧，但是观测到该点的关键帧数却不超过cnThObs帧，那么该点检验不合格

3.3. 新建mappoint

1. 在当前关键帧的Covisibility graph中找到共视程度最高的nn帧相邻帧存入vpNeighKFs。
2. 匹配当前关键帧未匹配的特征点与vpNeighKFs中未被匹配的特征点，并校验是否符合对级约束。
3. 三角化匹配成功的特征点对，得到其在世界坐标系中的坐标，生成一个空间点；
4. 检查其生成的空间点是否复合以下要求
   • 空间点投影到两相机坐标系后，其深度是否为正；
   • 视差是否小于阈值；
   • 空间点在两个相机的重投影误差是否小于阈值；
   • 尺度连续性是否一致；

3.4. 局部捆集调整（BA）

当LocalMapping已经处理完队列中的最后的一个关键帧，并且闭环检测此时没有请求停止LocalMapping；也就是说现在LocalMapping有空，那么就来一次local BA吧！

3.5. 剔除冗余关键帧

检测并剔除当前帧相邻的关键帧中冗余的关键帧
剔除的标准是：该关键帧的90%的MapPoints可以被其它至少3个关键帧观测到

4. LoopClosing回环检测

交给LocalMapping的关键帧几乎都会给LoopClosing。取出一个 LoopClosing队列里的关键帧为$K_i$

4.1. 获得候选关键帧

• 计算在Covisibility graph中与此节点$K_i$连接的节点（即关键帧），总的来说这一步是为了计算阈值 minScore；
• 在最低相似度 minScore的要求下，向mpKeyFrameDB索要可能形成闭环检测的候选帧集合；
• 对这些闭环检测候选帧进行连续性检测，剔除那些不满足连续性检测的闭环检测帧；

4.2. 计算相似转换（sim3）

在这一步进一步确定关键帧$K_i$和哪一个候选帧形成闭环检测；
并通过计算sim3确定关键帧$K_i$和闭环检测帧之间的位姿；