衢州交友:ORB-SLAM2 论文&代码学习 —— Tracking 线程

admin/2020-02-24/ 分类:科技/阅读:
JS设计模式——策略模式

转载请注明出处,谢谢 原创作者:MingruiYu 原创链接:https://www.cnblogs.com/MingruiYu/p/12352960.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 有了初步的匹配后,就会进入这个更精细的求解当前帧位姿的步骤 —— 局部地图跟踪。

局部地图里包括:

K8s 开船记-全站登船:Powered by .NET Core on Kubernetes

  • 和当前帧有共同观察到的 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 判断系统之前的跟踪状态。如果之前跟踪丢失,则要不断进行重定位 TrackingRelocalization(),直到当前帧与 KF Database 中的某个 KF 匹配上了。如果之前跟踪正常,则继续跟踪,一般来说使用 TrackingTrackWithMotionMode() 进行估计,但如果运动模型还未建立,或者刚刚进行了重定位,则使用 TrackingTrackReferenceKeyFrame() 进行估计。TrackReferenceKeyFrame() 指当前帧和其 Reference KF 进行匹配来估计位姿,其匹配的搜索量会大很多,所以当 TrackingTrackWithMotionMode() 不行的时候才会用它。

具体的位姿估计方式都是 匹配 优化。只是匹配的方式会有所不同:

  • Tracking::TrackReferenceKeyFrame() 是根据 BoW 来在当前帧所有提取出的 FeaturePoints 和 Reference Frame 的 MapPoints 中进行匹配(当然使用 BoW 有可以减少计算量的方法);
  • Tracking::TrackWithMotionMode() 中是有了位姿初值,所以可以根据该初值进行投影,将上一帧的 MapPoints 先投影至当前帧的一个大概区域,从而缩小了搜索的区域大小,减小了搜索量。

如果是 Localization 模式,那么如果之前系统跟踪丢失,同样不断进行重定位 Tracking::Relocalization()。如果之前系统跟踪正常,与 SLAM 模式不同的地方在于,其会判断当前处于 VO 情况还是正常情况:

  • 正常情况:与 SLAM 模式基本一致,根据运动模式是否已经建立而采用 TrackingTrackWithMotionMode() 或 TrackingRelocalization()。
  • VO 情况:与 SLAM 模式不一样了,此时进行 TrackingTrackWithMotionMode() 和 TrackingRelocalization(),优先使用 Relocalization() 的结果(此时重定位的结果更可靠一些),如果重定位失败,则采用 Tracking::TrackWithMotionMode() 继续跟踪甚至直接丢失,如果重定位成功,则可以推出 VO 模型回到正常模式。

局部地图跟踪

只有 SLAM 模式下,且上一步当前帧位姿初值估计成功(有位姿初值了)的情况下才会进行局部地图跟踪。

在局部地图跟踪优化后,会判断优化的效果如何,如果效果可以的话,才会判断本次跟踪成功(当前帧位姿初值估计 局部地图跟踪 都成功才算成功),否则本次跟踪丢失。

决定是否生成关键帧,并插入关键帧

如果当前帧丢失的话,那肯定是不会将其作为 KF 插入的。但刚初始化完没几帧就丢失了,说明初始化的质量不行,系统 Reset,重新初始化。(从中可以看出,ORB-SLAM2 对于初始化的质量标准很高,所以也经常出现在实际中其迟迟不肯启动的状况)。

如果当前帧跟踪成功,更新运动模型,且根据论文中的标准决定当前帧是否作为 KF 插入 Map,并送入 LocalMapping 线程。

ORB-SLAM2 系列博文

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

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

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

,

Sunbet

Sunbet www.006yb.com是Sunbet申博公司指定亚洲官方直营现金网,官方授权,老品牌信誉有保障.Sunbet携手上海市申博机械科技有限公司欢迎您加入我们。

TAG:
阅读:
广告 330*360
广告 330*360
Sunbet_进入申博sunbet官网
微信二维码扫一扫
关注微信公众号
新闻自媒体 Copyright © 2002-2019 Sunbet 版权所有
二维码
意见反馈 二维码