ORB-SLAM2源码阅读（三）——跟踪线程

目录

1.TRACK线程整体流程

2.恒速模型跟踪

应用场景

具体流程

 3.参考关键帧跟踪

具体流程

4. 重定位跟踪原理

应用场景 

思想

具体流程 

检测重定位候选关键帧

1.TRACK线程整体流程

ORB-SLAM2跟踪部分主要包括两个阶段，第一个阶段包括三种跟踪方法：用参考关键帧来跟踪、恒速模型跟踪、重定位跟踪，它们的目的 是保证能够“跟的上”，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该 帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。

跟踪线程流程图

 Track线程的主要在track中主要实现的。主要的实现步骤看流程图即可理解。其实总结一下就是如果跟踪正常，按恒速模型跟踪，如果不正常或者跟丢不断地使用关键帧或者重定位寻找。若实在不行只能重置。

2.恒速模型跟踪

对应函数Tracking::TrackWithMotionModel()，示意图如下：

应用场景

1. 用恒速模型先估计一个初始位姿

2. 用该位姿进行投影匹配 SearchByProjection ，候选点来自 GetFeaturesInArea ，未使用 BoW

3. BA 优化（仅优化位姿），提供比较粗糙的位姿

思想

1.假设短时间内（相邻帧）物体处于匀速运动状态，可以用上一帧的位姿和速度来估计当前帧的位姿。

2.移动模式跟踪 跟踪前后两帧 得到变换矩阵。

3.上一帧的地图 3d 点反投影到当前帧图像像素坐标上，在不同尺度下不同的搜索半径内，做描述子匹配搜索可以加快匹配。

4.在投影点附近根据描述子距离进行匹配（需要 >20 对匹配，否则匀速模型跟踪失败 , 运动变化太大时会出现这种情况），然后以运动模型预测 的位姿为初值，优化当前位姿

5.优化完成后再剔除外点，若剩余的匹配依然 >=10 对，则跟踪成功，否则跟踪失败，需要 Relocalization

具体流程

1 ：创建 ORB 特征点匹配器 最小距离 < 0.9* 次小距离 匹配成功

2：更新上一帧的位姿和地图点

单目：只计算了上一帧世界坐标系位姿就退出了。 Tlr*Trw = Tlw

3：使用当前的运动速度 ( 之前前后两帧位姿变换 ) 和上一帧的位姿来初始化 当前帧的位姿 R,t

4：在当前帧和上一帧之间搜索匹配点（ matcher.SearchByProjection)

（1）通过投影 ( 使用当前帧的位姿 R,t) ，对上一帧的特征点 ( 地图点 ) 进行跟踪 .

（2）上一帧 3d 点投影到当前坐标系下，在该 2d 点半径 th 范围内搜索可以匹配的匹配点

（3）遍历可以匹配的点，计算描述子距离，记录最小的匹配距离，小于阈值的，再记录匹配点特征方向差值

（4）进行方向验证，剔除方向差直方图统计中，方向差值数量少的点对，保留前三个数量多的点对。

5：如果找到的匹配点对如果少于 20 ，则扩大搜索半径 th=2*th, 使用 SearchByProjection() 再次进行搜索。

6：使用匹配点对对当前帧的位姿进行优化 G2O 图优化

7：如果 2d-3d 匹配效果差，被标记为外点，则当前帧 2d 点对于的 3d 点设置为空，留着以后再优化

8：根据内点的匹配数量，判断 跟踪上一帧是否成功。

 3.参考关键帧跟踪

对应函数 tracking::TrackReferenceKeyFrame()

应用场景 ：没有速度信息的时候、刚完成重定位、或者恒速模型跟踪失败后使用，大部分时间不用。只利用到了参考帧的信息。

1. 匹配方法是 SearchByBoW ，匹配当前帧和关键帧在同一节点下的特征点，不需要投影，速度很快

2. BA 优化（仅优化位姿），提供比较粗糙的位姿

思路 :

当使用恒速模型匹配到的特征点数较少时，就会选用关键帧模式跟踪。

思路是：尝试和最近一个关键帧去做匹配。为了快速匹配，利用了 bag of words （ BoW ）来加速匹配

具体流程

1.计算当前帧的BoW；

2.通过特征点的bow加快当前帧和参考帧之间的特征点匹配。使用函数matcher.SearchByBoW()。

（1）对属于同一node（同一node才可能是匹配点）的特征点通过描述子距离进行匹配，遍历该node 中特征点，特征点最小距离明显小于次小距离才作为成功匹配点，记录特征点对方向差统计到直方图   
（2）记录特征匹配成功后每个特征点对应的MapPoint（来自参考帧），用于后续3D-2D位姿优化
（3）通过角度投票进行剔除误匹配

3.将上一帧的位姿作为当前帧位姿的初始值（加速收敛），通过优化3D-2D的重投影误差来获得准确位姿。3D-2D来自第2步匹配成功的参考
帧和当前帧，重投影误差 e = (u,v) - project(Tcw*Pw)，只优化位姿Tcw，不优化MapPoints的坐标。

顶点 Vertex: g2o::VertexSE3Expmap()，初始值为上一帧的Tcw

边 Edge（单目）: g2o::EdgeSE3ProjectXYZOnlyPose()，一元边 BaseUnaryEdge

边 Edge （单目） : g2o::EdgeSE3ProjectXYZOnlyPose() ，一元边 BaseUnaryEdge

+ 顶点 Vertex ：待优化当前帧的 Tcw

+ 测量值 measurement ： MapPoint 在当前帧中的二维位置 (u,v)

+ 误差信息矩阵 InfoMatrix: Eigen::Matrix2d::Identity()*invSigma2( 与特征点所在的尺度有关 )

+ 附加信息： 相机内参数： e->fx fy cx cy

3d 点坐标 ： e->Xw[0] Xw[1] Xw[2] 2d 点对应的上一帧的 3d 点

优化多次，根据边误差，更新 2d-3d 匹配质量内外点标记，当前帧设置优化后的位姿。

4 剔除优化后的 outlier 地图点

lower_bound( begin,end,num) ：从数组的 begin 位置到 end-1 位置二分查找第一个大于或等于 num 的数字，找到返回该数字的地址，不存在

则返回 end 。通过返回的地址减去起始地址 begin, 得到找到数字在数组中的下标。

upper_bound( begin,end,num) ：从数组的 begin 位置到 end-1 位置二分查找第一个大于 num 的数字，找到返回该数字的地址，不存在则返

回 end 。通过返回的地址减去起始地址 begin, 得到找到数字在数组中的下标。

  

4. 重定位跟踪原理

对应函数Tracking::Relocalization()

应用场景 

跟踪丢失的时候使用，很少使用。利用到了相似候选帧的信息。

1. 用 BoW 先找到与该帧相似的候选关键帧（函数 DetectRelocalizationCandidates ）

2. 遍历候选关键帧，用 SearchByBoW 快速匹配，

3. 匹配点足够的情况下用 EPnP 计算位姿并取出其中内点做 BA 优化（仅优化位姿），

4. 如果优化完内点较少，通过关键帧投影生成新的匹配（函数 SearchByProjection ），

5. 对匹配结果再做 BA 优化（仅优化位姿）。

思想

当 TrackWithMotionModel 和 TrackReferenceKeyFrame 都没有跟踪成功，位置丢失后，需要在之前的关键帧中匹配最相近的关键帧，进

而求出位姿信息。

使用当前帧的 BoW 特征映射，在关键帧数据库中寻找相似的候选关键帧，因为这里没有好的初始位姿信息，需要使用传统的 3D-2D 匹配点的

EPnP 算法来求解一个初始位姿，之后再使用最小化重投影误差来优化更新位姿。

具体流程 

1 ：计算当前帧的 BoW 向量和 Feature 向量

2 ：在关键帧数据库中找到与当前帧相似的候选关键帧组

3 ：创建 ORB 特征点匹配器 最小距离 < 0.75* 次小距离 匹配成功。

ORBmatcher matcher(0.75,true);

4 ：遍历每一个候选关键帧使用 BOW 特征向量加速匹配，匹配太少的去掉，选择符合要求的候选关键帧用其地图点为其创建 pnp 优化器

5 ：使用 PnPsolver 位姿变换求解器 , 更加 3d-2d 匹配点

6 点直接线性变换 DLT, 后使用 QR 分解得到 R,t, 或者使用 (P3P) ， 3 点平面匹配算法求解。

这里会结合 Ransac 随采样序列一致性算法，来提高求解的鲁棒性。

6 ： EPnP 算法迭代估计姿态作为当前帧的初始位姿，使用最小化重投影误差 BA 算法来优化位姿

7 ：如果优化时记录的匹配点对内点数量少于 50 ，想办法再增加匹配点数量：通过投影的方式对之前未匹配的点进行 3D-2D 匹配，又给了一 次重新做人的机会

8 ： 如果新增的数量加上之前的匹配点数量 大于 50 ，再次使用 位姿优化算法进行优化

9 ：如果上面优化后的内点数量还比较少，还想挽留一下，就缩小搜索窗口重新投影匹配（比之前使用更多的地图点了），如果最后匹配次

数大于 50 ，就认为是可以勉强扶起来的阿斗，再给 BA 位优化一次。否则，放弃了（真的已经仁至义尽了！）

10 ：如果经过上面一系列的挽救操作，内点数量 大于等于 50 ，则重定位成功。

检测重定位候选关键帧

对应函数 DetectRelocalizationCandidates