ORBSLAM2系列-Tracking跟踪线程

参考关键帧跟踪

参考关键帧跟踪就是用参考关键帧的地图点来对当前普通帧进行跟踪，其中使用了词袋加速特征匹配。

• 当前普通帧图像的描述子转化为BoW向量
• 使用BoW加速特征匹配
• 使用重投影误差优化当前帧的位姿（没有地图点）
• 剔除优化后的外点

描述子转化为BoW向量

在程序开始时，会读取词典文件ORBvoc.txt，这里面存储的就是词袋数据，以树的形式存放（方便查找），下图中的各个节点都是描述子，计算图像中提取的描述子与下图中的节点的描述子，找到与图像描述子最相近的叶子节点，这就是将描述子转化为BoW向量。

一幅图像里所有特征点都会转化为两个map容器：BowVector 和 FeatureVector

词袋向量BowVector

词袋向量BowVector主要用于后面的回环检测，实际类型是std::map<WordId, WordValue>

• WordId：表示的是描述子在所有叶子节点中距离最近的叶子节点id
• WordValue：指的是该描述子对应的权重，也就是一幅图像中出现该节点的频率（权重是累加更新）

特征向量FeatureVector

特征向量FeatureVector主要用于加速特征匹配，实际类型是std::map<NodeId, std::vector<unsigned int> >

• NodeId：指的是距离叶子节点深度为leve up对应的NodeId（对应上图中的Word’s node id）
• 这个NodeId就是用于特征匹配的顶层节点，就是说搜索该特征点的匹配时，是寻找和它具有同样NodeId下面所有子节点中的Word进行匹配，搜索区域见图示中的 Word’s search region。因此这个搜索大小是根据level up确定的，level up值越大，搜索范围越广，速度越慢；level up值越小，搜索范围越小，速度越快，但能够匹配的特征就越少
• std::vector：指的是在该NodeId下所有特征点在图像中的索引

BoW加速特征匹配

考虑初始化时候的特征匹配：根据网格搜索一定范围的特征点，然后使用汉明距离计算特征点之间的描述子距离，选取最优和次优的特征点根据一系列筛选得到最好的结果。

使用BoW加速特征匹配其实只是替换了第一步（但是最终确定是否匹配依然还是要依靠描述子距离），根据前一帧特征点所在的NodeId，查找与该NodeId相同的后一帧特征点，将这些特征点作为候选匹配点，然后进行描述子距离。这样就是没有使用网格搜索，而是使用同一节点下的特征点匹配，加速了特征匹配的过程

这部分是SearchByBoW()函数，主要流程如下：

• 1.使用BoW向量中的FeatureVector容器，找到参考关键帧和普通帧相同的NodeId（表明可能是在同一类）
• 2.分别遍历参考关键帧和普通帧的FeatureVector->second （这是一个双循环），计算两两之间的描述子距离（汉明距离）
• 3.根据描述子距离，得到参考关键帧中每个特征点对应的最优和次优匹配
• 4.经过一系列筛选，得到了参考关键帧每个特征点的最优匹配点：
• • 4-1.最优匹配点的描述子距离要小于阈值
  • 4-2.最优匹配的描述子距离要明显优于次优匹配的描述子距离
  • 4-3.建立旋转直方图，剔除掉匹配点对的角度差不在前三个bin中的匹配点（就是不合群的点对）

需要注意的👇

• ⏩lower_bound(id)返回的是迭代器，指向大于等于id的第一个值。ORBSLAM2中使用这个函数在while循环中，找到两帧图像的NodeId相同的地方（就是对齐）
• ⏩对最优次优匹配点对的筛选过程中，与初始化的特征匹配中的筛选过程是一样的，具体可以参考那里

重投影误差优化

在优化之前，需要为优化的变量（当前帧位姿）设置一个初始值，这样有助于优化的收敛：

//将匹配点对送到mvpMapPoints，也就是当前帧的地图点
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
//当前帧的位姿是需要优化的变量，这里设置一个初始值：上一帧的位姿
mCurrentFrame.SetPose(mLastFrame.mTcw); 

重投影误差PoseOptimization就是将地图点的3D坐标投影到当前帧，得到了当前帧的像素坐标（这就是预测值），那么它应该与在当前帧原本匹配的点的像素坐标（观测值）的差为0，然而事实并不如此，观测值与预测值之间总是有个误差，因此我们就使用最小二乘方法是的这个误差趋近于0：

$$e^{\ast }=argmin\frac{1}{2}{\Sigma }_{i=1}^n||u_i-\frac{1}{s_i}KT{P}_i|{|}_2^2$$

优化之后的位姿，就是跟踪后当前帧的位姿。

剔除外点

在PoseOptimization函数中一共进行了4次优化，每一次优化之后都会去掉一些外点，这些外点不参与到下一次的优化过程中，但是在函数中没有对这些外点进行剔除，因此需要在函数运行之后把这些点剔除掉

外点：在优化过程中需要计算卡方值chi2，如果这个点的卡方值大于5.991（对于单目来说，设置的卡方自由度为2的阈值），就认为这个点是外点。这个意义指的是，这个点的重投影误差太大（好像是相差一个像素点？），会影响到优化的收敛，因此需要去掉（也有可能是误匹配）

恒速跟踪模型

大部分时间都是使用这个跟踪函数TrackWithMotionModel，就是使用上一帧的地图点匹配当前帧的特征点，然后进行优化，整体流程跟上面参考帧跟踪除了特征匹配的方法不同，其他差不多。

为什么叫恒速跟踪？因为是假设短时间内（相邻帧）物体处于匀速运动状态，因此可以使用上一帧的位姿和速度来估计当前帧的位姿（这需要涉及到速度）

• 使用恒速假设进行初始位姿估计
• 对上一帧地图点进行投影匹配
• 使用重投影误差优化当前帧的位姿（没有地图点）
• 剔除优化后的外点

初始位姿估计

假设恒定速度，用这个固定的速度和上一帧位姿就可以得到当前帧位姿。在程序中使用的mVelocity作为两帧之间相对变换（也就是速度的意义），它的计算是在上一次跟踪后赋值的：

//注意，这是上一帧的赋值，
//1.也就是说这里的mCurrentFrame指的是上一帧，LastTwc指的是上上一帧
//2.仔细看下标，Tcw是世界坐标系转化成相机坐标系，而Twc是反过来的，这两个相乘就是上一帧和上上一帧之间的相对变换
mVelocity = mCurrentFrame.mTcw*LastTwc; 

如下图所示，可以得到当前帧的估计位姿：

需要注意的👇

• ⏩上面的代码行中，在计算LastTwc变量时，是将 $R_{wc}$ 放到4x4矩阵的左上方，而将 $mOw$ （光心坐标）放在了右边。为什么要这样放置？ 首先需要知道这里要求的是 $T_{wc}$ 是有相机坐标系转换为世界坐标系（与我们正常的转换反过来），也就是说 $T_{cw}^{-1}=T_{wc}$ ，由于旋转矩阵 $R$ 是正交矩阵：转置和逆相同，就得到如下的形式。而下面公式中 $-R_{cw}^T{t}_{cw}$ 刚好就是世界坐标系中相机光心的坐标，也就是 $mOw$

$$T_{cw}=\left[\begin{array}{cc}{R}_{cw}& t_{cw}\\ 0^T& 1\end{array}\right]\Rightarrow T_{wc}=T_{cw}^{-1}=\left[\begin{array}{cc}{R}_{cw}^T& -R_{cw}^T{t}_{cw}\\ 0^T& 1\end{array}\right]$$

投影匹配

不同于初始化时候的特征匹配，这时候的匹配是使用的前一帧地图点（3D）到后一帧特征点（2D）的匹配，因此可以将3D地图点按照初始估计的位姿投影到2D图像上，再使用网格搜索匹配点，这就是投影匹配SearchByProjection函数的主要内容：

• 1.提取前一帧每个地图点，将其按照相机成像模型，投影到后一帧的图像上，坐标为 $(u^{\prime },v^{\prime })$
• 2.在坐标 $(u^{\prime },v^{\prime })$ 的后一帧图像上进行网格搜索（就是在r半径范围内搜索），与初始化时候的特征匹配一样，选出候选匹配点
• 3.计算描述子距离，在候选匹配点中选出最优的匹配点、
• 4.经过一系列筛选（跟前面一样）：
• • 4-1.描述子距离要小于最小阈值
  • 4-2.建立旋转直方图，剔除掉匹配点对的角度差不在前三个bin中的匹配点（就是不合群的点对）

优化与剔除外点

这两部分与上面参考帧跟踪一样，都是只优化当前帧位姿，然后剔除一些超出阈值的点

重定位

重定位是在跟丢了的情况下才会运行的（比如图像模糊造成的跟踪点太少），相当于在重新找定位，所以，整个过程就是在不断的寻找当前帧和其他帧之间的关系，希望能够找到匹配关系，再计算当前帧位姿。应该说这是挽回程序跑丢了的最后一环。

• 当前普通帧图像的描述子转化为BoW向量
• 使用词袋寻找与当前帧相似的候选关键帧组
• 寻找候选关键帧与当前帧之间的匹配
• 使用EPnP算法进行位姿估计
• 使用重投影误差优化当前帧的位姿（没有地图点）
• 如果内点较少，就使用投影匹配，再次优化

描述子转化为BoW向量

这部分与上面一样，不介绍

寻找候选关键帧组

这部分是DetectRelocalizationCandidates函数中。在关键帧数据库中（所有的关键帧），使用词袋向量进行搜索，寻找与当前帧的Word数量最多的关键帧，这部分使用了一些策略进行候选关键帧的筛选：

• 1.使用BowVector中的WordId，寻找与当前帧具有公共单词WordId的所有关键帧
• • 1-1.使用的倒排索引，mvInvertedFile[i]表示包含了第i个WordId的所有关键帧，因此遍历BowVector可以得到所有与当前帧具有公共单词的关键帧
  • 1-2.pKFi->mnRelocWords存放的是pFKi关键帧与当前帧共同具有的单词数量
• 2.找到与当前帧共同单词最多的单词数maxCommonWords，设定一个最小单词数的阈值：minCommonWords = maxCommonWords*0.8f
• 3.将能够达到最小公共单词数阈值minCommonWords的关键帧存储，并计算其与当前帧之间的相似度得分（这个得分使用第三方库中BoW2中的score计算）
• 4.提取这些达到最小公共单词数阈值的关键帧的共视程度最高的10个关键帧为1组，并计算这一组的相似度得分总和
• 5.找到所有组的最高得分，设定一个最小的得分阈值：minScoreToRetain = 0.75f*bestAccScore
• 6.将能够达到最小得分阈值的组中的最高得分那个关键帧作为候选关键帧，对每个组这样提取，就得到了候选关键帧组

BoW加速特征匹配

这部分与参考关键帧跟踪中的一样，这里是对候选关键帧中每个关键帧与当前帧进行的BoW匹配。如果匹配点对数量太少，就跳过这个候选关键帧，反之就准备进行EPnP算法求解位姿

EPnP算法求解位姿

经过了上面的匹配过程，对匹配点数量足够的关键帧依次进行EPnP算法求解位姿

重定位中求解位姿R,t

重投影误差优化

对求解之后的内点进行g2o优化，优化的过程跟上面跟踪方法中使用的重投影误差一样，这算是倒数第二次的挽回了。

如果这次优化之后内点数量小于10（太少了），那么就跳过这个关键帧（没法挽回），优化下一个关键帧，否则就删除外点

如果这次优化之后内点数量小于50（有点少），那么就进行下一步，也就是还能有点挽回余地

需要注意的👇

• ⏩这次优化过程中使用的地图点是在EPnP算法求解位姿时得到的内点（就是匹配点），而对于那些没有匹配的点就是最后的救命稻草，也就是下一步

最后的投影匹配

最后进行的投影匹配是使用关键帧的地图点（除了上面优化时候使用的内点）投影到当前帧，然后再进行网格搜索（这个过程与初始化时候一样），得到能够匹配点（这个投影匹配是SearchByProjection函数，整体过程也是与局部地图跟踪一样）

在重定位的最后，根据条件一共运行了两次投影匹配+重投影误差优化，都是为了能够找回跟踪，如果某一候选关键帧完成了整个重定位过程，那么就不再计算其他的候选关键帧（这部分主要看程序，逻辑简单，主要就是SearchByProjection+PoseOptimization两个函数不断的挽救跟踪）

局部地图跟踪

局部地图跟踪是在上面三个跟踪方法之后必须要运行的，就是从局部地图中找到更多的匹配关系，然后对当前帧的位姿进行再一次的优化。

对于当前普通帧来说，当前帧的局部地图就是它的一些共视关键帧，然后从这些共视关键帧中的地图点与当前帧特征点做匹配，就会得到更多的匹配关系，用于后面对位姿的优化。（这主要是由于上面的三种跟踪方法主要是去跟踪某一帧的地图点，而局部地图是使用很多帧的地图点去匹配）

• 更新局部关键帧和局部地图点
• 筛选地图点做投影匹配
• 使用重投影误差优化当前帧的位姿（没有地图点）
• 剔除优化后的外点

其实整体做法跟恒速跟踪模型有点像，只不过需要寻找更多的匹配关系

更新局部地图

局部地图就是拿到与当前帧有共视关系的一些关键帧和这些关键帧的地图点

• 找到与当前帧有共视关系的关键帧，存放在mvpLocalKeyFrames中
• • 遍历当前帧的地图点（之前跟踪的点）,找到能够观测到这些点的帧，并按照共视点多少存放，这就是一级共视帧
  • 遍历一级共视帧，找到与一级共视帧共视程度最好的10个关键帧，这就是二级共视帧
  • 遍历一级共视帧，找到该共视帧的父子关键帧（就是共视程度最大的关键帧）
  • 将上面的关键帧存放在mvpLocalKeyFrames中
• 遍历局部关键帧中的所有地图点，存放在mvpLocalMapPoints中

投影匹配

局部地图点需要经过一系列的筛选才能用到特征匹配中：

• 1.将当前帧的地图点做标记，就是说这些点不需要做投影了，因为之前跟踪过程已经投影过
• 2.遍历所有局部地图点，判断其是否在当前视野范围内：
• • 2-1.该地图点投影到相机坐标系，深度值需要为正数
  • 2-2.该地图点投影到像素坐标系，该像素点需要在图像的边界范围内
  • 2-3.计算相机中心到地图点的距离，这个距离需要在有效范围内（这个有效范围是在地图点生成时估计的这个地图点的最大深度和最小深度）
  • 2-4.计算相机观测该点与该店平均观测方向的夹角，小于60°才能说被观测到
  • 2-5.根据光心到地图点的距离，预测会在哪个金字塔层找到该点匹配

• • 2-6.最后将符合要求的点标记好，mbTrackInView变量是用来标记这个点可以用来投影
• 3.如果可以用来投影的点的数量大于0，就可以进行投影匹配了，这个投影匹配过程跟之前差不多，对投影的点进行网格搜索
• • 3-1.提取预测地图点的预测金字塔层，设定搜索半径r，这个半径是根据地图点的视角决定，视角越小，半径越小
  • 3-2.使用网格搜索，限定在预测的金字塔层搜索，得到候选匹配点
  • 3-3.遍历候选匹配点，计算两两之间描述子距离，选出最优和次优的匹配点
  • 3-4.最优描述子需要小于设定阈值，并且最优次优需要要满足阈值比例
  • 3-5.最后得到了匹配点对，存放在帧的mvpMapPoints中

优化与剔除外点

这两部分与上面参考帧跟踪一样，都是只优化当前帧位姿，然后剔除一些超出阈值的点

插入关键帧

判断是否插入关键帧

经过上面的跟踪过程，接下来就会判断此普通帧是否可以作为关键帧插入，需要一系列的决策：

• 纯定位模式不插入关键帧（纯定位模式只进行跟踪，没有后面的处理）
• localmapping线程如果被闭环检测使用，就不插入关键帧
• 距离上次重定位比较近，并且地图中关键帧数目超出最大值，不插入关键帧？
• c1a：很长时间没有插入关键帧，可以插入
• c1b：满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入
• c2：和参考帧相比当前跟踪的点数量太少，同时跟踪的内点不能太少，这时候需要插入了
• (c1a || c1b) && c2 = true 此时插入关键帧

新建关键帧

此时，需要将此普通帧封装成关键帧，然后传入到localmapping线程处理：

//使用此函数想localmapping线程插入关键帧
mpLocalMapper->InsertKeyFrame(pKF);

其中，使用了列表mlNewKeyFrames ，将关键帧插入此列表中，用于Tracking线程和LocalMapping线程之间信息的传递