ORB-SLAM2 论文&代码学习 —— LocalMapping 线程
转载请注明出处，谢谢
原创作者：Mingrui
原创链接：https://www.cnblogs.com/MingruiYu/p/12360913.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 了。
