ORB-SLAM2 论文&代码学习 —— Tracking 线程

转载请注明出处，谢谢
原创做者：MingruiYu
原创连接：http://www.javashuo.com/article/p-amuyogdu-hb.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 线程。

ORB-SLAM2 系列博文

ORB-SLAM2 初体验 —— 配置安装

ORB-SLAM2 论文&代码学习 —— 概览

ORB-SLAM2 论文&代码学习 —— Tracking 线程