ORB-SLAM2源码学习（六）：相机跟踪（局部地图跟踪和关键帧创建）

目录

1.局部地图跟踪

1.1 更新局部关键帧UpdateLocalKeyFrames

1.2 更新局部地图点（来自局部关键帧）UpdateLocalPoints()

1.3 投影匹配

2. 对比四种跟踪方式以及使用的投影匹配

3.关键帧创建

3.1 判断是否需要创建新关键帧: NeedNewKeyFrame()

3.2 关键帧的创建CreateNewKeyFrame()

1.局部地图跟踪

当初始位姿估计成功（参考关键帧来跟踪、恒速模型跟踪、重定位跟踪跟踪成功）后需要进行局部地图跟踪TrackLocalMap()。

初始位姿估计只是跟踪一帧得到初始位姿，局部地图跟踪搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的MapPoints后进行位姿优化。

bOK = TrackLocalMap();  

具体流程：

1. 更新局部地图，包括局部关键帧和地图点;UpdateLocalKeyFrames() UpdateLocalPoints()
2. 对局部MapPoints进行投影匹配 SearchLocalPoints()
3. BA优化位姿（和初始位姿估计调用优化函数相同）

1.1 更新局部关键帧UpdateLocalKeyFrames

• 一级共视关键帧：KF1、KF2是F的一级共视关键帧
• 二级共视关键帧：KF1、KF2的共视关键帧(虚线框内)是F的二级共视关键帧
• 当前帧：mCurrentFrame
• 参考关键帧: 与当前帧共视程度最高的关键帧作为参考关键帧，mCurrentFrame.mpReferenceKF 在KeyFrame::UpdateConnections() 里确定关键帧的父子关系
• 父关键帧：和当前关键帧共视程度最高的关键帧
• 子关键帧：是上述父关键帧的子关键帧

局部关键帧的组成：

1. 当前地图点的所有共视关键帧（邻居）。
2. 1中所有关键帧共视关系前10大的共视关键帧（邻居的邻居）。
3. 1中所有关键帧的父子关键帧（邻居的父母和孩子）。

最后将与当前帧共视关系最强的关键帧设为参考关键帧mpReferenceKF

1.2 更新局部地图点（来自局部关键帧）UpdateLocalPoints()

局部关键帧能够观测到的所有地图点，称为局部地图点。

1.3 投影匹配

将局部地图点进行投影匹配，得到更多的匹配关系(目的)。注意，局部地图点中已经是当前帧地图点的不需要再投影，只需要将此外的并且在视野范围内的点和当前帧进行投影匹配。

具体流程：

1. 遍历当前帧的地图点，标记这些地图点不参与之后的投影搜索匹配；
2. 判断所有局部地图点中除当前帧地图点外的点，是否在当前帧视野范围内；（是否在视野范围内有个单独函数 isInFrustum()）
3. 如果需要进行投影匹配的点的数目大于0，就进行投影匹配，增加更多的匹配关系。SearchByProjection()

2. 对比四种跟踪方式以及使用的投影匹配

①参考关键帧跟踪

通过词袋模型计算参考关键帧和当前帧的匹配特征点（没有用投影匹配），上一帧的位姿作为当前帧的初始位姿，然后进行BA求两帧之间的位姿变换。

应用场景：没有速度信息的时候、刚完成重定位、或者恒速模型跟踪失败后使用，大部分时间不用。只利用到了参考帧的信息（本质上是尝试和最近一个关键帧去做匹配）。

②恒速模型跟踪

首先根据上上帧到上一帧的位姿，把上一帧的特征点像素坐标映射到当前帧，然后在映射后结果坐标周围进行搜索，找到与之匹配的特征点；根据速度得到当前帧的初始位姿，然后进行BA求两帧之间的位姿变换。

使用了投影匹配，由于恒速模型可以计算当前帧相对于上一帧的平移向量，根据相机的前进/后退来约束搜索尺度范围（在投影点多大的范围内进行匹配）。此外它是对上一帧的有效地图点（过滤外点）进行投影的。

投影匹配流程：1. 计算帧间平移 → 2. 投影上一帧地图点 → 3. 运动方向约束搜索尺度 → 4. 匹配并记录角度差 → 5. 旋转直方图剔除

尺度范围：

在进行投影匹配的时候会给定特征点的搜索范围,考虑到处于不同尺度(也就是距离相机远近,位于图像金字塔中不同图层)的特征点受到相机旋转的影响不同，因此会希望距离相机近的点的搜索范围更大一点,距离相机更远的点的搜索范围更小一点,所以要在这里,根据点到关键帧/帧的距离来估计它在当前的关键帧/帧中，会大概处于哪个尺度。

应用场景

大部分时间都用这个跟踪，只利用到了上一帧的信息。

③重定位

当TrackWithMotionModel 和 TrackReferenceKeyFrame 都没有跟踪成功，位置丢失后，需要在之前的关键帧中匹配相近的关键帧，然后使用3D-2D匹配点的 EPnP算法求初始位姿，之后再使用BA来优化位姿。

具体而言：用词袋找到与当前帧相似的候选关键帧（可能找到多个），再然后通过BoW加速计算每一个候选关键帧和当前帧的匹配特征点（这个过程和 参考关键帧跟踪中当前帧和参考帧匹配特征点一样），使用EPnP依次求初始位姿。每个位姿结果使用BA优化，选择内点数量大于50的作为初始位姿。

如果内点数量不够多呢？就通过投影匹配方式对之前未匹配的点进行匹配，再进行BA优化求解。由于进行重定位，上一帧没有意义，基于地图点距离预测尺度范围。

投影匹配流程：1. 遍历局部关键帧地图点 → 2. 过滤已匹配点 → 3. 投影并预测尺度 → 4. 多层级搜索 → 5. 最佳匹配筛选 → 6. 旋转直方图剔除

④局部地图跟踪

搜索局部关键帧、局部地图点，和当前帧进行投影匹配，得到更多匹配的地图点后进行位姿优化，初始位姿根据前面三种方法计算的到。

具体而言：根据当前帧的地图点来找能观测到当前帧的一级共视关键帧，将这些一级共视关键帧的二级关键共视帧、子关键帧、父关键帧一起作为局部关键帧；局部关键帧中所有的地图点作为局部地图点；局部地图点（去掉当前帧的地图点 和 不在当前帧视野范围内无效的地图点）投影到当前帧进行特征点匹配，匹配结果进行BA优化。

投影匹配的时候通过视角余弦（RadiusByViewingCos）和预测尺度（nPredictedLevel）调整搜索尺度范围。

总结：

四种跟踪方式对比：

相同点：本质上都是计算匹配点对以及初始位姿，然后使用BA进行位姿优化。

不同点：求解匹配特征点方法和当前帧的初始位姿设置不同

• 参考关键帧跟踪通过BoW计算当前帧和参考关键帧的匹配点对；初始位姿使用上一帧的位姿；
• 恒速运动模型通过投影匹配计算匹配点对；初始位姿通过速度（这里的速度是上一帧到当前帧的位姿Tcl）确定；
• 重定位 通过BoW计算每一个相似关键帧和当前帧 的匹配点对；初始位姿通过EPnP计算；
• 局部地图跟踪是将 局部地图点（来自局部关键帧）投影到当前帧计算匹配点对；初始位姿由前面三种方法确定。

3.关键帧创建

关键帧是取局部相近帧中最有代表性的一帧

选取的指标主要有：

1. 距离上一关键帧的帧数是否足够多（时间）。比如我每隔固定帧数选择一个关键帧，这样编程简单但效果不好。比如运动很慢的时候，就会选择大量相似的关键帧，冗余，运动快的时候又丢失了很多重要的帧。
2. 距离最近关键帧的距离是否足够远（空间）/运动。比如相邻帧根据pose计算运动的相对大小，可以是位移也可以是旋转或者两个都考虑，运动足够大（超过一定阈值）就新建一个关键帧，这种方法比第一种好。但问题是如果对着同一个物体来回扫就会出现大量相似关键帧。
3. 跟踪局部地图质量（共视特征点数目）。记录当前视角下跟踪的特征点数或者比例，当相机离开当前场景时（双目或比例明显降低）才会新建关键帧，避免了第2种方法的问题。缺点是数据结构和逻辑比较复杂。

if(NeedNewKeyFrame())
    CreateNewKeyFrame();

3.1 判断是否需要创建新关键帧: NeedNewKeyFrame()

是否生成关键帧,需要考虑以下几个方面:

1. 最近是否进行过重定位，重定位后位姿不会太准，不适合做参考帧；
2. 当前系统的工作状态: 如果LocalMapping线程还有很多KeyFrame没处理的话,不适合再给它增加负担了；
3. 距离上次创建关键帧经过的时间: 如果很长时间没创建关键帧了的话,就要抓紧创建关键帧了；
4. 当前帧的质量: 当前帧观测到的地图点要足够多，同时与参考关键帧的重合程度不能太大。

3.2 关键帧的创建CreateNewKeyFrame()

注：双目和RGBD创建新的地图点，单目不会（单帧没有尺度）

具体流程：

1. 将当前帧构造成关键帧；
2. 将当前关键帧设置为当前帧的参考关键帧；
3. 对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作；
4. 关键帧插入到链表 mlNewKeyFrames中，等待local mapping线程处理

思维导图：