ORB-SLAM2 论文&代码学习 —— Tracking 线程
转载请注明出处，谢谢
原创作者：Mingrui
原创链接：https://www.cnblogs.com/MingruiYu/p/12352960.html
本文要点：
- ORB-SLAM2 Tracking 线程 论文内容介绍
- ORB-SLAM2 Tracking 线程 代码结构介绍
写在前面
上一篇文章中我们已经对 ORB-SLAM2 系统有了一个概览性的了解。通过我绘制的详细的思维导图形式的程序导图，我们也可以很清晰地看出各个线程之间的关系，以及它们是如何和论文中的 System Overview 图对应上的。
依旧祭出该图，方便查看：
也再次献上我绘制的程序导图全图：ORB-SLAM2 程序导图
从这篇文章开始，我们将会进入 ORB-SLAM2 的每个部分，学习 ORB-SLAM2 每个部分的具体结构和逻辑。Tracking 线程是 ORB-SLAM2 系统的主线程，每一帧图像送入后也会先经过 Tracking 线程的处理。所以这篇文章，我们先来看看 Tracking 线程的具体工作。
老规矩，还是分两部分：以 ORB-SLAM 论文为参考 和 以 ORB-SLAM2 代码（程序导图）为参考。
以 ORB-SLAM 论文为参考
首先，来看看论文中对 Tracking 线程的介绍。
Tracking 线程的主要工作如下：
- 对于新读取的帧，提取 ORB 特征
- （系统初始化）
- 当前帧位姿初值估计（根据上一帧 + motion-only BA，或进行重定位）
- 局部地图跟踪
- 对上一步得到的位姿初值进行进一步 BA 优化
- 局部地图：指 Covisibility Graph 中附近的 KFs 及其 MapPoints 所组成的局部的地图
- 决定是否将当前帧作为关键帧插入 LocalMapping 线程
下面我们具体来看这些内容。
注： Tracking 线程中很重要的一个工作是进行单目初始化，这一部分我会单独写一片文章来进行介绍，所以本文会暂时跳过单目初始化的具体内容。
ORB 特征提取
ORB-SLAM2 系统采用 ORB 特征作为贯穿整个系统使用的特征提取和描述方式。其优势在于，提取速度快（大幅快于 SIFT 和 SURF，但其实 ORB 特征的提取还是整个系统中最耗时的部分）。关于 ORB 特征的详细内容可见论文：ORB: An efficient alternative to SIFT or SURF PDF。
ORB 特征具有旋转不变性，但没有尺度不变性。为了减小尺度变化对于 ORB 特征的影响，ORB-SLAM 采用尺度金字塔的方式，将图像放大或缩小形成不同尺度（共8个，每个尺度之间的缩放比例为1.2），之后再在每个尺度的图像上都提取一遍 ORB 特征（提出 ORB 特征会带有一个标记，标记其是从哪个尺度提取出来的），将每个尺度提取出的 ORB 特征汇总在一起，就成为了该图像提取的 ORB 特征。
为了尽可能使得 提取的 ORB 特征在图像上分布均匀（ORB 特征提取本身存在一个问题，其在图像上分布不均，经常有的局部一大堆特征点，有的局部没有特征点），ORB-SLAM 将每个尺度的图像，划分成一个个小格格（切蛋糕了），在每个小格格上提取至少5个特征点。如果提取不出5个特征点，就将提取特征的阈值放低一些。
提取的 ORB 特征在 ORB-SLAM 系统中相当重要，会贯穿整个系统，用于所有的特征匹配。
当前帧位姿初值估计
Tracking 线程的目的之一是求出当前帧的位姿，其巧妙地将这个求解过程分为两步，从粗到细。相对较粗的步骤 —— 当前帧位姿初值估计，在估计好一个初值后，会进入相对较细的步骤 —— 局部地图跟踪，然后得到一个最终的位姿（当然在 LocalMapping 线程中还要继续优化）。
首先来看这个相对较粗的步骤 —— 相机位姿初值估计。其有三种可能的估计方式，论文里提到了两种：根据上一帧和运动模型进行估计（上一帧跟踪成功） 和 通过全局重定位估计（上一帧跟踪丢失）。还有一种是根据 Reference KF 进行估计（虽然上一帧跟踪成功，但因为种种原因，无法使用上一帧和运动模型进行估计），我们会在代码部分对其进行介绍。另外，在这一部分中，除了会估计当前帧的位姿外，还会将当前帧的 FeaturePoints 和 MapPoints 做一个初步的匹配。
根据上一帧和运动模型进行估计
如果上一帧跟踪成功，就可以继续正常的跟踪。ORB-SLAM 系统假设了一个匀速运动模型，意思就是假设当前帧与上一帧之间的相对位姿变化量 = 上一帧和上上帧之间的相对位姿变化量。通过这个可以先估计出当前帧的一个位姿初值，根据这个位姿初值，将上一帧的 MapPoints 和当前帧的 FeaturePoints 进行匹配，之后根据匹配进行优化。（如果没有找到足够多的匹配，就要使用上面提到的 根据 Reference KF 进行估计的方法了）
重定位
如果上一帧跟踪失败了，没有上一帧的位姿，肯定是无法通过上面的方法继续跟踪的，所以要进行重定位。重定位的含义就是从 KF Database 中寻找有没有哪个 KF 与当前帧很相似，有的话可能当前帧就在那个 KF 附近，从而定位了当前帧。
寻找可能的 KF 并计算当前帧位姿的方法如下：根据当前帧的 FeaturePoints 计算当前帧的 BoW。通过当前帧的 BoW 与 KF Database 中的 KFs 的 BoW 进行匹配，初步筛选初一批 Candidate KFs，并将当前帧的 FeaturePoints 与 Candidate KFs 含有的 MapPoints 进行匹配。之后，RANSAC 迭代计算当前帧的位姿（通过 PnP 算法求解）。注意，上述计算的目的之一是进一步筛选 Candidate KFs，所以根据每一个 Candidate KFs 都要计算出一个当前帧的位姿，直到找到一个合适的 Candidate KF，根据它计算出的当前帧位姿很合适（有足够多的 inliers）。再对其进行进一步优化，再进行更多的 FeaturePoints 和 MapPoints 的匹配。如果再优化后这个位姿计算还很合适（有足够多的 inliers），那就确定当前帧的位姿了，之后就可以继续正常跟踪了。
局部地图跟踪
当在上一步中获得了当前帧的位姿初值并且当前帧的 FeaturePoints 和 MapPoints 有了初步的匹配后，就会进入这个更精细的求解当前帧位姿的步骤 —— 局部地图跟踪。
局部地图里包括：
- 和当前帧有共同观察到的 MapPoints 的 KFs
*上述 KFs 的 Covisible KFs - 它们含有的某些 MapPoints
- 该 MapPoint 可以投影到当前帧的画幅内
- 该 MapPoint 的平均可视方向与当前帧的方向的夹角不大于60度
- 该 MapPoint 距离当前帧光心的距离在一定范围内（范围太大的话，ORB 特征的尺度不变性很难保证，这样匹配出错的概率很大）
在计算得到局部地图的同时，还需要尽可能进一步地将局部地图的 MapPoints 与 当前帧还未匹配的 FeaturePoints 相匹配。最终，对该局部地图进行 BA 优化。
决定是否将当前帧作为 KeyFrame
如果当前帧比较重要，则会将其作为 KF 插入 Map 并送入 LocalMapping 线程。ORB-SLAM 的一个特点就是，其插入 KF 的条件很宽松，这样会插入很多 KFs，而 ORB-SLAM 会在 LocalMapping 线程中对它们中冗余的进行剔除。这样的目的是不放过可能有用的帧，增强系统的鲁棒性，以应对纯旋转等很难处理的相机运动。
虽然这个条件很宽松，但还是有条件的：
- 距离上一次重定位已经过去了超过20帧
- LocalMapping 线程正闲置，但如果已经有连续20帧内没有插入过 KF 了，那么 LocalMapping 线程不管忙不忙，都要插入 KF 了 （忙就给老子停下hhh）
- 当前帧至少追踪了 50 个点（当前帧质量不能太差）
- 当前帧追踪的点中不能有90%以上和其 Reference KF 相同（当前帧不能太冗余）
以 ORB-SLAM2 代码（程序导图）为参考
看完了论文，可能有点不好理解，毕竟用文字进行描述的难度是很高的，特别是 ORB-SLAM2 系统内部各部分之间的逻辑又是较为复杂的。Talk is cheap, give me code. 下面我们从 ORB-SLAM2 的代码出发，结合我绘制的 ORB-SLAM2 程序导图，进一步加深对 Tracking 线程的理解。
如上图所示，在 System.cc 的主循环中，启动了对于 Tracking 线程的调用。每次读入一帧图像，为其创建 Frame 对象，在创建的同时就提起了 ORB 特征。这里有一个细节，在单目初始化时，对于该帧提取的 ORB 特征数是平时的两倍。
如上图所示，进入 Tracking 线程中，可以看到一个很重要的状态变量为 mState，根据它来区分当前系统的状态。当系统还未初始化时，会运行 MonocularInitialization() 来进行初始化。关于这一部分会在后面的博文中专门介绍。
初始化之后，就会进入常规 Tracking 过程，其中首先进行当前帧位姿，之后进行局部地图跟踪，再之后决定是否生成 KF，并插入 KF。下面我们一个部分一个部分来看。
因为这些地方实在不好截图，请大家点击这张导图的链接 （在文首）来查看清晰大图吧。
注：
ORB-SLAM2 系统有两种模式（可以由使用者手动切换），其以 mbOnlyTracking 变量进行区分：
- SLAM 模型：所有线程都正常工作
- Localization 模式：只有 Tracking 线程工作，其它线程均不工作
同时，Localization 模式中也有两种情况（系统自动判定，根据当前跟踪情况自动切换），其以 mbVO 变量进行区分：
- VO 情况：Visual Odometry，上一帧追踪到的点大部分是 VO 点（未能与 MapPoints 匹配（此处存疑）），此时不会进入局部地图跟踪，直到重定位成功，具体后面细说。
- 正常情况：常规，所有部分正常运行。
当前帧位姿初值估计
如果是 SLAM 模式，则首先根据 mState 判断系统之前的跟踪状态。如果之前跟踪丢失，则要不断进行重定位 Tracking::Relocalization()，直到当前帧与 KF Database 中的某个 KF 匹配上了。如果之前跟踪正常，则继续跟踪，一般来说使用 Tracking::TrackWithMotionMode() 进行估计，但如果运动模型还未建立，或者刚刚进行了重定位，则使用 Tracking::TrackReferenceKeyFrame() 进行估计。TrackReferenceKeyFrame() 指当前帧和其 Reference KF 进行匹配来估计位姿，其匹配的搜索量会大很多，所以当 Tracking::TrackWithMotionMode() 不行的时候才会用它。
具体的位姿估计方式都是 匹配 + 优化。只是匹配的方式会有所不同：
- Tracking::TrackReferenceKeyFrame() 是根据 BoW 来在当前帧所有提取出的 FeaturePoints 和 Reference Frame 的 MapPoints 中进行匹配（当然使用 BoW 有可以减少计算量的方法）；
- Tracking::TrackWithMotionMode() 中是有了位姿初值，所以可以根据该初值进行投影，将上一帧的 MapPoints 先投影至当前帧的一个大概区域，从而缩小了搜索的区域大小，减小了搜索量。
如果是 Localization 模式，那么如果之前系统跟踪丢失，同样不断进行重定位 Tracking::Relocalization()。如果之前系统跟踪正常，与 SLAM 模式不同的地方在于，其会判断当前处于 VO 情况还是正常情况：
- 正常情况：与 SLAM 模式基本一致，根据运动模式是否已经建立而采用 Tracking::TrackWithMotionMode() 或 Tracking::Relocalization()。
- VO 情况：与 SLAM 模式不一样了，此时进行 Tracking::TrackWithMotionMode() 和 Tracking::Relocalization()，优先使用 Relocalization() 的结果（此时重定位的结果更可靠一些），如果重定位失败，则采用 Tracking::TrackWithMotionMode() 继续跟踪甚至直接丢失，如果重定位成功，则可以推出 VO 模型回到正常模式。
局部地图跟踪
只有 SLAM 模式下，且上一步当前帧位姿初值估计成功（有位姿初值了）的情况下才会进行局部地图跟踪。
在局部地图跟踪优化后，会判断优化的效果如何，如果效果可以的话，才会判断本次跟踪成功（当前帧位姿初值估计 + 局部地图跟踪 都成功才算成功），否则本次跟踪丢失。
决定是否生成关键帧，并插入关键帧
如果当前帧丢失的话，那肯定是不会将其作为 KF 插入的。但刚初始化完没几帧就丢失了，说明初始化的质量不行，系统 Reset，重新初始化。（从中可以看出，ORB-SLAM2 对于初始化的质量标准很高，所以也经常出现在实际中其迟迟不肯启动的状况）。
如果当前帧跟踪成功，更新运动模型，且根据论文中的标准决定当前帧是否作为 KF 插入 Map，并送入 LocalMapping 线程。
