ORB-SLAM2 论文&代码学习 —— LocalMapping 线程
MingruiYu 人气:0转载请注明出处,谢谢
原创作者: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 了。
ORB-SLAM2 系列博文
ORB-SLAM2 初体验 —— 配置安装
ORB-SLAM2 论文&代码学习 —— 概览
ORB-SLAM2 论文&代码学习 —— Tracking 线程
ORB-SLAM2 论文&代码学习 —— 单目初始化
ORB-SLAM2 论文&代码学习 —— LocalMapping 线程
加载全部内容