ORB-SLAM3源码阅读笔记1:Tracking、LocalMapping和LoopClosing三线程之间的关系

Jul 25,2021   5277 words   19 min

Tags: SLAM

最近在修改ORB-SLAM3的代码,因为有时候改着改着就改不下去了,所以就阅读了一下源码,记了一些阅读笔记。这里也分享出来,如果有不对的地方,欢迎提出。

1.对象的建立

Tracking、LocalMapping、LoopClosing对象均在System构造函数(System())中创建。

Tracking

LocalMapping

LoopClosing

2.对象间关系的创建

在System类构造函数中通过SetTracker()SetLocalMapper()SetLoopClosing()三个函数将建立好的三个对象彼此之间互相关联(例如,在Tracking中可以直接通过LocalMapper类的成员变量mpLocalMapper修改LocalMapping线程中的一些变量或者调用其函数)。如果有可视化,则通过SetViewer()函数实现关联。整理关系如下图所示。

3.线程的并行运行

整个系统有三个核心线程TrackingLocalMappingLoopClosing,其中Tracking线程就是主线程无需单独启动。而LocalMapping和LoopClosing是在主线程中被新建并启动的

Tracking线程

LocalMapping线程

LoopClosing线程

对于LocalMapping和LoopClosing线程而言,其启动的都是各自的Run()函数,其中包含了实现Mapping和Closing必要的步骤。在System构造函数执行后,LocalMapping和LoopClosing线程同时被启动。此时,Tracking线程(主线程)、LocalMapping和LoopClosing线程同时存在,不断运行。随着影像的输入,Tracking线程不断进行位姿估计并改变系统的状态与变量。同时在运行的LocalMapping和LoopClosing线程不断检查当前系统的状态和变量是否满足条件,只要满足条件,就会执行相应内容。对于LocalMapping线程而言,间隔为3秒(usleep(3000)),对于LoopClosing线程,间隔为5秒(usleep(5000))。三个线程之间的关系可以简单如下图所示。而如果LoopClosing线程中判定当前已经执行完回环或者地图融合,就会启动一个新的优化线程Optimizer进行全局优化。

4.线程的结束

整个系统通过System的成员函数Shutdown()进行结束。对于Tracking、LocalMapping和LoopClosing这三个线程,首先通过LocalMapping和LoopClosing各自的RequestFinish()成员函数分别结束,如果有可视化,再结束可视化窗口。对于Tracking线程,由于是主线程,所以当程序结束的时候就自动结束,无需手动管理。 进一步,在成员函数RequestFinish()中,其实只是将类的Flag成员变量mbFinishRequested设为True。在不断执行的Run()函数中,会通过CheckFinish()函数检查mbFinishRequested的状态,如果这个变量设置为True,就会退出Run()函数,从而结束线程。

5.线程间的变量传递

如前文所述,三个线程之间通过对共享的变量进行赋值与查询,实现不同状态、流程的切换。

(1)Tracking与LocalMapping

在LocalMapping线程中,核心的执行函数是Run()。在Run()函数中,通过对多个和Tracking类共享的变量进行查询,判断是否需要进行LocalMapping。整个Run()函数主要由一个不断遍历的“死循环”构成,如下图所示。 而为了保证可以正常退出,通过对不同状态的判别,通过break跳出循环,从而结束函数。在这个大循环中,是否执行LocalMapping的核心判断条件是CheckNewKeyFrames() && !mbBadImu。只要满足了这个条件,就会执行核心的LocalMapping。

对于CheckNewKeyFrames()函数,它的作用就是检查是否有新的关键帧输入进来(关键帧列表是否为空,因为LocalMapping每次处理完一个关键帧就会将其从列表中删除)。具体而言,它会返回一个bool类型的结果,如下图所示。当执行这个函数的时候,首先会有一个线程锁。然后返回LocalMapping的成员变量mlNewKeyFrames是否为空。empty()函数用于判定一个vector是否为空,如果为空返回true,否则返回false。所以如果为空,CheckNewKeyFrames()返回false,否则返回true。 在LocalMapping所有成员函数中,使用它的地方如下。 可以看到,CheckNewKeyFrames()函数主要用于对当前状态进行判断。进一步,该函数判断的核心变量是mlNewKeyFrames。该变量是CheckNewKeyFrames()函数的核心。对于此变量,在LocalMapping中它又在以下地方被修改了。 而另一个判断变量mbBadImu用于表达当前IMU状态是否是坏的。它同样是LocalMapping的成员变量。如果IMU状态OK,它就为false,否则为true。它最初在LocalMapping的构造函数中被赋初值为false。在整个LocalMapping类的成员函数中,使用它的地方如下: 可以看到,对于LocalMapping而言,对mlNewKeyFramesmbBadImu的修改主要在InsertKeyFrame()Run()函数中。这两个函数,除了Run()一直独立于Tracking线程运行外,InsertKeyFrame()主要在Tracking线程中被调用。具体而言,在Tracking类中,主要在以下函数中调用了InsertKeyFrame()函数: 可以看到,前两个函数主要在初始化阶段被调用,而最后一个函数则是有可能在一次运行中多次反复调用。具体而言,CreateNewKeyFrame()Track()函数中被调用,如下图所示。 Tracking中调用LocalMapping的一些变量和函数: LocalMapping中调用Tracking的一些变量和函数: 所以,Tracking和LocalMapping之间共享变量以及修改关系如下图所示。 Tracking的Track()函数与LocalMapping的Run()函数核心执行流程如下所示。

(2)Tracking与LoopClosing

与LocalTracking类似的,在LoopClosing线程中,核心的执行函数是Run()。在Run()函数中,通过对多个和Tracking类共享的变量进行查询,判断是否需要进行LoopClosing。整个Run()函数主要由一个不断遍历的“死循环”构成,如下图所示。 而为了保证可以正常退出,通过对不同状态的判别,通过break跳出循环,从而结束函数。在这个大循环中,是否执行LoopClosing的核心判断条件是CheckNewKeyFrames()NewDetectCommonRegions()这两个函数。只要满足了这两个条件,就会执行核心的LoopClosing。

对于CheckNewKeyFrames()函数,它的作用就是检查回环关键帧list是否为空。具体而言,它会返回一个bool类型的结果,如下图所示。 当执行这个函数的时候,首先会有一个线程锁。然后返回LoopClosing的成员变量mlpLoopKeyFrameQueue是否为空。empty()函数用于判定一个vector是否为空,如果为空返回true,否则返回false。所以如果为空,CheckNewKeyFrames()返回false,否则返回true。在LoopClosing所有成员函数中,只有在Run()函数中使用到了它。

可以看到,CheckNewKeyFrames()函数主要用于对当前状态进行判断。进一步,该函数判断的核心变量是mlpLoopKeyFrameQueue。该变量是CheckNewKeyFrames()函数的核心。对于此变量,它又在以下地方被修改了。 而另一个判断核心函数是NewDetectCommonRegions(),它用于告诉系统,是否检测到了新的公共区域。这个函数只在Run()函数中被调用了一次。它也是整个LoopClosing的核心函数之一,包含对当前状态的多个判断,从而进入不同分支。由于ORB-SLAM3中有多地图的概念,所以包含地图融合回环这两个方面的内容。具体而言,该函数的核心流程如下。 一个完整的Run()函数执行流程如下。 Tracking中调用LoopClosing的一些变量和函数 LoopClosing中调用Tracking的一些变量和函数 所以,Tracking和LoopClosing之间共享变量以及修改关系如下图所示。

(3)LocalMapping与LoopClosing

LocalMapping与LoopClosing是独立于Tracking的两个独立线程,分别用于局部建图与回环检测。在各自的类成员变量中,分别有mpLocalMappermpLoopCloser指针,以此方便调用对方的一些函数和成员变量。它们彼此之间并没有直接的启动调用关系,如之前的线程图所示,它们都是通过Tracking这个主线程对状态的改变来决定是否启动的。

LocalMapping中调用LoopClosing的地方: LoopClosing中调用LocalMapping的地方: 进一步可以画出调用关系图如下所示。 另外,也正如ORB-SLAM3设计的,在执行完LoopClosing之后,就会进行全局BA。这个功能是在RunGlobalBundleAdjustment()函数中实现的。 更进一步,是通过调用Optimizer的GlobalBundleAdjustment()(无IMU)FullInertialBA()(有IMU)实现的。进一步,我们可以在LoopClosing中找到调用RunGlobalBundleAdjustment()函数的地方,如下。 都是以新建线程方式调用,如下所示。 程序新建了优化线程以后,其就会自动执行相关代码。这期间不会停止,线程会一直运行,直到优化完成,线程结束,退出。

最后需要注意的是,RunGlobalBundleAdjustment()函数返回的是一个bool变量,用于说明优化是否执行成功。在LoopClosing中通过mbFinishedGBAmbRunningGBAmbStopGBA控制。 具体流程如下:

首先,在LoopClosing对象创建的时候,mbFinishedGBA为True、mbRunningGBA为False、mbStopGBA为False。然后我们启动新的线程用于优化,启动之前,这三个变量分别被修改为mbRunningGBA为True、mbFinishedGBA为False、mbStopGBA为False。在我们启动的Run函数中,会修改这几个变量。如果执行成功,mbFinishedGBA会设为True、mbRunningGBA会设为False。最终执行结束,退出。

最后再简单说明一下mbStopGBA在哪些地方被修改了。这个变量最重要的作用就是用于控制优化是否进行,如下所示。在MergeLocal()MergeLocal2()CorrectLoop()之前都会先检查是否在运行优化线程,如果运行了,就想办法把它停掉。 首先,程序会通过isRunningGBA()函数访问成员变量mbRunningGBA进行判别,判断当前是否在金乡全局优化,如果正在进行全局优化,就先把mbStopGBA设为True,表示要停止优化。然后会调用变量mpThreadGBA来看当前现状的状态,如果当前线程不为空(mpThreadGBA在构造函数中被赋为NULL),也就表示已经启动了优化线程,就和该线程脱离detach(),然后删除该线程成员变量。detach()的作用是将子线程和主线程的关联分离,也就是说detach()后子线程在后台独立继续运行,主线程无法再取得子线程的控制权,即使主线程结束,子线程未执行也不会结束。当主线程结束时,由运行时库负责清理与子线程相关的资源。

本文作者原创,未经许可不得转载,谢谢配合

返回顶部