ORB SLAM2 Tracking部分论文及代码解读

ORB SLAM2是在ORB SLAM基础上提出的一种性能十分优异的SLAM算法，主要通过tracking（跟踪）、local mapping（本地制图）、loopclosing（闭环检测）三个并行线程，流程如下图所示。本篇博客主要针对tracking线程展开。

代码解读

这里我们直接开始看代码，通过解析代码来更明确详细的阐述ORB SLAM2 tracking部分的内容。
以单目mono_tum.cc文件为例。首先

main函数前面一大段都在读取图片和相关数据信息，然后由SLAM.TrackMonocular()跳入跟踪线程。

SLAM.TrackMonocular()在system.cc文件中。首先检查是否为单目，而后检查初始化状态，进而跳入GrabImgeMonocular()函数。

GrabImgeMonocular()函数在Tracking.cc文件中。首先将RGB或者RGBA图像转换成灰度图像，而后分别构造用于初始化和正常运行下的关键帧，进而跳到Track()函数中。

首先判断是否需要初始化，由于我们以单目为例，假设需要初始化，跳转至MonocularInitialization()函数中。

如果没有单目初始器，则构造单目初始器。

如果单目已经创建，判断当前帧特征点数量是否大于100，如果不满足，则重新构造初始器。然后跳入SearchForInitialization()函数。

SearchForInitialization()函数在ORBmatcher.cc文件里，主要作用是遍历初始化时的当前帧和参考帧间潜在匹配点，并使DescriptorDistance()函数计算两帧间潜在匹配点距离，通过对比更新找到最优和次优匹配距离，并且更新和检查旋转直方图，最后返回匹配成功的对数。

重新回到Tracking.cc里的MonocularInitialization()函数，如果初始化时两帧之间的匹配点太少，则重新初始化，用于单目相机产生的图片缺少深度信息，故需要三角化匹配点（其作为Initialize()函数的一个参数），然后跳入Initialize()函数。

Initialize()函数在Initializer.cc文件中，主要作用是保存所有的初始化时匹配上的特征点及其索引号，同时分别计算单应矩阵(homography matrix)和基础矩阵(fundamental matrix)及其相应得分，然后运用下图所示的公式计算$R_H$：

当$R_H>0.45$时选择单应矩阵，否则选择基础矩阵。

再次回到Tracking.cc里的MonocularInitialization()函数中，删除无法进行三角化的匹配点，并且得到世界坐标到相机坐标的变换矩阵。，然后进入CreateInitialMapMonocular()函数。

通过关键点初始化地图点(map point)并初始化地图。

至此，单目初始化全部完成。

回到Track()函数中，Update()函数用来将跟踪线程的数据拷贝到绘图线程。

接下来开始跟踪部分

CheckReplacedInLastFrame()函数对上一帧中被替换的地图点进行检查，然后如果跟踪失败或者当前帧紧跟着重定位后的帧，则调用TrackReferenceKeyFrame()函数。

通过ComputeBoW()函数将当前帧的描述子转化为BoW向量，从而在后续特征点匹配过程中加速匹配，然后通过SearchByBow()函数实现当前帧和参考关键帧的特征点匹配。

SearchByBow()函数在ORBmatcher.cc文件中，首先通过pKF->GetMapPointMatches()获取当前关键帧的具体的地图点，再生成特征点角度旋转差统计用的直方图。使用KFit->second和Fit->second分别取出参考关键帧和当前帧在同一节点下ORB特征点，然后通过遍历KF（参考关键帧）中属于该结点的特征点，并分别通过vpMapPointsKF[realIdxKF]和pKF->mDescriptors.row(realIdxKF)获得KF中该特征对应的MapPoint和描述子。接着，同理，使用F.mDescriptors.row(realIdxF)获得F(当前帧)中该特征对应的描述子，进而使用DescriptorDistance()函数计算参考关键帧和当前帧针对同一特征的描述子之间的距离，并用其更新最优（最小）和次优（倒数第二小）距离。

然后根据阈值和角度投票剔除误匹配。

回到Track()函数中，如果跟踪成功，则进入TrackWithMotionModel()函数。

首先用UpdateLastFrame()函数更新一下上一帧信息， 用mCurrentFrame.SetPose()估计当前帧的位姿，用fill()函数清空当前帧的地图点，而后跳入SearchByProjection(）函数中。

SearchByProjection(）函数在ORBmatcher.cc文件中，首先生成特征点角度旋转差统计用的直方图，然后得到当前帧的位姿估计并判断相机是前进还是后退。

遍历上一帧中有效的地图点，对上一帧中有效的地图点进行跟踪，并将其投影到当前帧中，通过判断相机是前进、后退还是其他来判断特征点搜索区域。

这段和SearchByBow()函数类似，再次不再赘述。

回到TrackWithMotionModel()函数中，如果跟踪点过少（小于20），则扩大所搜半径（阈值扩大为先前的两倍）。

如果发现还是不能获得足够的特征点。就认为跟踪失败了，而后优化位姿，剔除有外点的地图点，将成功匹配到的地图点进行累加求和，然后判断是否为纯定位模式，如果是则置位mbV0标志。

如果恒速度模型跟踪失败，则启用参考帧跟踪模型。如果初始化不成功，则只能重定位。
如果处于定位模式，即mbOnlyTracking标志位为1，则只进行跟踪，不执行建图。在此模式下，如果跟踪失败，进行重定位，如果没有跟丢，并且此帧匹配了较多的地图点，如果其速度不为0，即mVelocity不为空，则执行恒速度模型，如果恒速度模型失败，则通过参考关键帧定位。

思想类似，可参考注释，不再赘述。

注：代码注释参考：https://github.com/DreamWaterFound/self_commit_ORB-SLAM2