ORBSLAM2解析（2）——Tracking 线程

主要参考

双目ORB-SLAM2代码个人总结（一） -> 非常详细描述了跟踪线程
ORB SLAM2源码解读 -> 整个系列按各个类（Frame、MapPoint、KeyFrame…）来解读

Tracking线程

Tracking线程作为系统的主线程，主要实现输入的预处理、地图的初始化、相机位姿估计、局部地图的跟踪、关键帧的判断与创建。（注意：文章以RGBD输入下的定位且建图模式进行分析）

输入的预处理

在图像每一帧的初始化中，ORBSLAM2中将双目或者RGB-D相机输入的数据都转化成了Stereo Keypoint，即$(u_L,v_L,u_R)$，这样后续对于双目或者RGB-D的处理是几乎一样的。

地图的初始化

地图初始化是发生在SLAM系统运行的最开始时候，创建一个能提取足够特征点的关键帧作为第一帧，并将第一帧固定，创建特征点对应的地图点。

相机位姿的估计（包括重定位）

相机位姿估计应该是整个跟踪线程最重要的部分了，这里重点讲述下个人的理解。这部分主要分为三种情况：跟踪运动模型、跟踪关键帧、运动丢失后重定位。（每部分思维图就不展开了，不然太长了）

《视觉SLAM十四讲》中对于PnP问题的求解是这么下结论的：在SLAM中，通常做法是先使用P3P/EPnP等方法估计相机位姿，再构建最小二乘优化问题对估计值进行调整（即进行BA）。Tracking线程中对于相机的估计主要分两步：先对相机的位姿做一个初步的预测，然后再利用BA对相机的位姿进行调整。

跟踪运动模型、跟踪关键帧、运动丢失后重定位三种情况下对于相机位姿的初步预测是不一样的，但有了预测的相机位姿后都是进行一次BA对相机位姿进行调整。具体情况如下：

1. 对于跟踪运动模型，要求相机的运动速度不为0，且假设相机是匀速运动的，即每两帧之间位姿的变化速度是相同的，因此可以通过 当前帧位姿 = 上一帧速度 × 上一帧位姿 来进行预测，具体的那行代码是mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw) 。然后再将上一帧的地图点重新投影至当前帧并与当前帧关键点匹配，matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR)。
2. 当相机的速度为0，跟踪模型的运动假设显然是不成立了，此时直接假设 当前帧位姿 = 上一帧位姿。此时，如果仍将上一帧的地图点重新投影至当前帧并与当前帧关键点匹配，显然是不合适。（可以认为将上一帧位姿直接作为当前帧位姿这个预测可能存在比较大的误差，投影匹配的方式只是在一个小范围的窗口进行搜索匹配，这样可以节省计算资源，但是需要在预测已经比较准确的情况下。）因此，可以选择跟踪上一帧的参考关键帧（如果还是匹配上一帧，那么后续BA优化将不起作用），并使用暴力匹配关键帧地图点与当前帧特征点的ORB，这过程可以通过词袋模型的k-d树结构进行加速，SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches)。此外，对于刚完成重定位的情况，个人认为需要有比较准确的地图点与特征点之间的匹配，因此也使用暴力匹配的方式。
3. 当相机运动丢失后，不存在上一帧位姿去估计当前帧位姿的可能了，因此使用PnPsolver（EPnP、ICP、RANSAC）求解相机的位姿作为优化前的预测值。在PnPsolver求解前，对于候选关键帧的地图点与当前帧特征点匹配使用SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i])；在首次优化后，如果nGood小于50大于10，还会使用重投影的方式SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100)进行地图点与特征点的匹配，然后再次进行优化，直到nGood够50，即有足够的匹配数量。

局部地图的跟踪

在位姿估计的时候，也进行了对上一帧或者参考关键帧的地图点的跟踪，但是这种情况下当前帧特征点没有与其它的地图点进行匹配。因此，跟踪局部地图的目的就是为了使当前帧的特征点能与更多的地图点进行匹配。

关键帧的判断与创建

这一步创建的关键帧主要是用于后续的局部建图和回环检测。