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

转载请注明出处，谢谢
原创做者：Mingrui
原创连接：http://www.javashuo.com/article/p-erxcszco-dx.html

---

本文要点：

• ORB-SLAM2 LocalMapping 线程 论文内容介绍
• ORB-SLAM2 LocalMapping 线程 代码结构介绍

写在前面

以前的 ORB-SLAM2 系列文章中，咱们已经对 Tracking 线程和其中的单目初始化部分进行了介绍。咱们将在本文中，对 ORB-SLAM2 系统的 LocalMapping 线程进行介绍。

依旧祭出该图，方便查看：

也再次献上我绘制的程序导图全图：ORB-SLAM2 程序导图

老规矩，仍是分两部分：以 ORB-SLAM 论文为参考 和 以 ORB-SLAM2 代码（程序导图）为参考。

以 ORB-SLAM 论文为参考

LocalMapping 线程的大体步骤以下：

• 接收从 Tracking 线程插入的 KF，并进行预处理
• 剔除质量较差的 MapPoints
• 经过三角化生成新的 MapPoints
  • Current KF 未与现有 MapPoints 匹配的 FeaturePoints 与其 Covisible KFs 的 FeaturePoints 进行匹配，并三角化
• Local BA
• 剔除冗余的局部 KF

LocalMapping 线程的存在主要有这么几个意义：

• 筛选 KFs
• 进一步优化 Tracking 线程获得的 KFs 位姿以及 MapPoints 坐标，但这个优化仍是相对轻量级的（与 LoopClosing 线程相比），且这里的优化不涉及回环

下面咱们对每个步骤进行详细的介绍。

插入 KF

当 Tracking 线程肯定一个要插入的 KF 时，实际上它并无真的完成将 KF 插入 Map 的动做。当咱们将一个 KF 插入 Map 中时，咱们须要同时作不少更新工做：

• 更新 Covisibility Graph（在 Covisibility Graph 中添加新的 KF node，根据共视关系添加新的 edge）
• 更新生成树
• 计算新 KF 的 BoW （便于后面经过特征匹配和三角化生成新的 MapPoints）

剔除质量较差的 MapPoints

存储在 Map 中的 MapPoints 须要有较高的质量（追踪良好，三角化正确），因此此处须要采起一些措施去掉质量较差的 MapPoints。判断 MapPoints 质量较差的标准为，在该 MapPoint 被创造后的3个 KFs 时间范围内：

• 实际看到该 MapPoints 的帧数 / 应该看到该 MapPoints 的帧数 < 25% （注意不仅是 KFs）
  • 应该看到该 MapPoints 的帧：当咱们有了某 MapPoint，也有了某帧的位姿时，咱们能够经过投影判断该 MapPoint 是否在该帧的视野内，这些帧就是应该看到该 MapPoints 的帧
  • 实际看到该 MapPoints 的帧：经过各类匹配方式，该 MapPoints 与某帧的某个 FeaturePoint 匹配上了，这些帧就是实际看到该 MapPoints 的帧
• 该 MapPoints 在被创造后，未能被至少3个 KFs 观测到（此处论文表达彷佛不太清楚）

注意：即便 MapPoints 在创造知足上述要求，得以保留，但不表明它们之后不可能被剔除。若是以后由于 KF 的剔除（下文会讲）致使观测到该 MapPoint 的 KF 数少于3个，或者在 local BA 中该观测被认为是 outlier，那么它依然会被剔除。

经过三角化生成生成新的 MapPoints

对于单目 ORB-SLAM 来讲，整个系统中只有两处能够在 Map 中添加 MapPoints：一处是初始化的时候，另外一处就是这里。

ORB-SLAM 将在 Current KF 的未能与已存在 MapPoints 匹配上的 FeaturePoints，与其 Covisible KFs 中一样未能与已存在 MapPoints 匹配上的 FeaturePoints 进行匹配。若是匹配上了，则能够经过三角化，生成一个新的 MapPoint（生成以后要检查其位置，视差，重投影偏差，尺度一致性）。这个 FeaturePoint 与 FeaturePoint 之间的匹配是经过 BoW 搜索实现的。

经过两个 KFs 生成新的 MapPoint 后，还要检查它有没有在别的 KFs 中出现。因此要将该 MapPoint 投影至其余的 Covisible KFs，与它们的 FeaturePoints 进行匹配。匹配上的话就将该 MapPoint 与那个 KF 的那个 FeaturePoint 连接上。

局部 BA

对 Current KF 及其 Covisible KFs 及其它们所观察到的全部 MapPoints 进行 BA 优化。

注意，其余观测到这些 MapPoints，可是再也不上述 KFs 之列的 KFs，也会做为约束参与该优化（其自己不会被优化）。

剔除冗余的局部 KF

在 Tracking 线程中，ORB-SLAM 以很是宽松的条件，向 Map 中插入了不少不少 KFs，但显然 Map 中是不能永久保留这么多 KFs 的，这会使 Map 过于庞大，且极大增长各类 BA 的运算量。因此要剔除一些信息冗余的。

若是 Current KF 及其 Covisible KFs 中，有哪一个 KF 它所观测到的 90% 的 MapPoints 都能被其它至少3个（尺度相同或更好的）KFs 观测到，则这个 KF 的信息就算做是冗余的，就把它去掉。这样作的目的是让 Map 的 KF 数不要太多，且在规模必定的场景内，Map 中的 KF 数目不要无上限的增加。这样也有利于减轻 BA 优化的负担。

以 ORB-SLAM2 代码（程序导图）为参考

上图就是 LocalMapping 线程的程序导图，从中能够很清晰地看出 LocalMapping 线程的逻辑，而且和论文中的步骤进行对应。

若是嫌这张图不够清晰的话，能够点击 ORB-SLAM2 程序导图连接（文首）查看清晰全图

插入 KF

在插入 KF 后，会经过 LocalMapping::SetAcceptKeyFrames(false) 通知 Tracking 线程，LocalMapping 线程正忙。记得在 Tracking 线程中最后一步决定是否插入关键帧时，有一个条件就是：

• LocalMapping 线程正闲置，但若是已经有连续20帧内没有插入过 KF 了，那么 LocalMapping 线程无论忙不忙，都要插入 KF 了

另外，LocalMapping 线程经过维护一个队列来存储 Tracking 线程送入，但还未被 LocalMapping 处理的 KFs。LocalMapping::CheckNewKeyFrames() 用来检查该队列里有没有 KF。

从上述队列中取出队首 KF，使用 LocalMapping::ProcessNewKeyFrame() 对其进行处理，包括计算该 KF 的 BoW，以及更新 Covisibility Graph。最后，通过上述处理的 KF 才能够真正插入 Map 之中。

剔除质量较差的 MapPoints

LocalMapping::MapPointCulling()

经过三角化生成新的 MapPoints

LocalMapping::CreateNewMapPoints()

MapPoints 融合

当队列中全部的 KFs 都通过上述处理了（队列空了），那么才会开始接下来的步骤。

MapPoints 融合，这部分实际上是属于经过三角化生成新的 MapPoints 里的，论文中说过：“经过两个 KFs 生成新的 MapPoint 后，还要检查它有没有在别的 KFs 中出现。因此要将该 MapPoint 投影至其余的 Covisible KFs，与它们的 FeaturePoints 进行匹配。匹配上的话就将该 MapPoint 与那个 KF 的那个 FeaturePoint 连接上”，这一步的目的就在与完成这项工做。

可是，这里须要注意，在上述表述中，“匹配上的话就将该 MapPoint 与那个 KF 的那个 FeaturePoint 连接上”，但若是这些条件都么知足，但那个 FeaturePoint 已经连接上了某个 MapPoint 怎么办？ORB-SLAM 采起的策略很简单，用新的 MapPoint 替换掉原来连接的 MapPoint。

举一个可能出现这种状况的情景：同时有4个刚送入 LocalMapping 线程的 KFs 观测到了 MapPoint_1 （MapPoint_1 此前未在 Map 中建立）。在上文三角化的过程当中，假设 KF_1 和 KF_2 三角化生成了 MapPoint_1，但同时 KF_3 和 KF_4 也三角化生成了 MapPoint_1。队列中全部 KFs 处理完毕后，此时，我在将 KF_1 的 MapPoint 投影至 KF_3 时，就会发现 KF_3 的匹配 FeaturePoint 已经连接了 MapPoint了。此时须要一个融合策略（ORB-SLAM 简单的采用了替换的方法）。

Local BA

当队列中全部的 KFs 都通过上述处理了（队列空），且 其余线程没有让 LocalMapping 线程暂停（后面会提到 LoopClosing 线程中有地方会让 LocalMapping 线程中的 Local BA 先暂停），则进行 Optimizer::LocalBundleAdjustment()。

剔除冗余的 KFs

LocalMapping::KeyFrameCulling()

最后经过 LocalMapping::SetAcceptKeyFrames(true) 通知 Tracking 线程，LocalMapping 线程闲下来了，能够有条件的接收 KFs 了。

ORB-SLAM2 系列博文

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

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

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

ORB-SLAM2 论文&代码学习 —— 单目初始化

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