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 论文&代码学习 —— 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 代码(程序导图)为参考

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

上图就是 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 线程

上一篇:POJ 1033 Defragment


下一篇:rhel6.4 using centos source