ORB-SLAM2详解（四）跟踪

本人邮箱：sylvester0510@163.com，欢迎交流讨论，
欢迎转载，转载请注明网址http://blog.csdn.net/u010128736/

这一部分是ORB_SLAM系统中最基本的一步，会对每一帧图像进行跟踪计算。Tracking线程运行在主线程中，主要思路是在当前帧和（局部）地图之间寻找尽可能多的对应关系，来优化当前帧的位姿。每次新采集到一帧图像，就是用下列接口将图像传入SLAM系统就行处理。该代码位于主程序中：

    // Pass the image to the SLAM systemSLAM.TrackMonocular(im,tframe);

在检查完系统将模式切换为跟踪模式后，是用下面接口进入功能：

mpTracker->GrabImageMonocular(im,timestamp);

一、ORB提取

本文做匹配的过程中，是用的都是ORB特征描述子。先在8层图像金字塔中，提取FAST特征点。提取特征点的个数根据图像分辨率不同而不同，高分辨率的图像提取更多的角点。然后对检测到的特征点用ORB来描述，用于之后的匹配和识别。跟踪这部分主要用了几种模型：运动模型（Tracking with motion model）、关键帧（Tracking with reference keyframe）和重定位（Relocalization）。

二、从前一帧初始化位姿估计

在成功与前面帧跟踪上之后，为了提高速率，本文使用与之前速率相同的运动模式来预测相机姿态，并搜索上一帧观测到的地图点。这个模型是假设物体处于匀速运动，例如匀速运动的汽车、机器人、行人等，就可以用上一帧的位姿和速度来估计当前帧的位姿。使用的函数为TrackWithMotionModel()。这里匹配是通过投影来与上一帧看到的地图点匹配，使用的是matcher.SearchByProjection()。

if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{bOK = TrackReferenceKeyFrame();
}
else
{bOK = TrackWithMotionModel();if(!bOK)bOK = TrackReferenceKeyFrame();
}

当使用运动模式匹配到的特征点数较少时，就会选用关键帧模式。即尝试和最近一个关键帧去做匹配。为了快速匹配，本文利用了bag of words（BoW）来加速。首先，计算当前帧的BoW，并设定初始位姿为上一帧的位姿；其次，根据位姿和BoW词典来寻找特征匹配，使用函数matcher.SearchByBoW()；最后，利用匹配的特征优化位姿。

三、通过全局重定位来初始化位姿估计

假如使用上面的方法，当前帧与最近邻关键帧的匹配也失败了，那么意味着需要重新定位才能继续跟踪。重定位的入口如下：

bOK = Relocalization();

此时，只有去和所有关键帧匹配，看能否找到合适的位置。首先，计算当前帧的BOW向量，在关键帧词典数据库中选取若干关键帧作为候选。使用函数如下：

// Relocalization is performed when tracking is lost// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisationvector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

其次，寻找有足够多的特征点匹配的关键帧；最后，利用RANSAC迭代，然后使用PnP算法求解位姿。这一部分也在Tracking::Relocalization() 里。

四、局部地图跟踪

通过之前的计算，已经得到一个对位姿的初始估计，我们就能透过投影，从已经生成的地图点中找到更多的对应关系，来精确结果。函数入口为：

bOK = TrackLocalMap();

为了降低复杂度，这里只是在局部图中做投影。局部地图中与当前帧有相同点的关键帧序列成为K1，在covisibility graph中与K1相邻的称为K2。局部地图有一个参考关键帧Kref∈K1，它与当前帧具有最多共同看到的地图云点。针对K1, K2可见的每个地图云点，通过如下步骤，在当前帧中进行搜索:
（1）将地图点投影到当前帧上，如果超出图像范围，就将其舍弃；
（2）计算当前视线方向向量v与地图点云平均视线方向向量n的夹角，舍弃n·v < cos(60°)的点云；
（3）计算地图点到相机中心的距离d，认为[dmin, dmax]是尺度不变的区域，若d不在这个区域，就将其舍弃；
（4）计算图像的尺度因子，为d/dmin；
（5）将地图点的特征描述子D与还未匹配上的ORB特征进行比较，根据前面的尺度因子，找到最佳匹配。
这样，相机位姿就能通过匹配所有地图点，最终被优化。

五、关键帧的判断标准

最后一步是确定是否将当前帧定为关键帧，由于在Local Mapping中，会剔除冗余关键帧，所以我们要尽快插入新的关键帧，这样才能更鲁棒。这个部分代码为：

 // Check if we need to insert a new keyframe
if(NeedNewKeyFrame())CreateNewKeyFrame();

确定关键帧的标准如下：
（1）在上一个全局重定位后，又过了20帧；
（2）局部建图闲置，或在上一个关键帧插入后，又过了20帧；
（3)当前帧跟踪到大于50个点；
（4）当前帧跟踪到的比参考关键帧少90%。

六、代码架构