ORB-SLAM3源码阅读笔记5:IMU相关优化函数及RECENTLY_LOST状态简单分析

Nov 11,2021   5330 words   20 min

Tags: SLAM

在ORB-SLAM3中,引入了IMU,因此自然也少不了与IMU相关的优化。因此本文简单整理ORB-SLAM3中与IMU相关的优化函数,以方便以后查阅。

1.暴力查找

与之前版本类似的,ORB-SLAM3中所有跟优化相关的函数都放在了Optimizer中。所以我们可以直接在Optimizer.h中搜索inertial关键词进行暴力查找,找到所有和IMU相关的函数,如下。 可以看到搜索到了12个匹配结果,进一步分析可以看到,主要是以下6个函数:

  • FullInertialBA()
  • PoseInertialOptimizationLastKeyFrame()
  • PoseInertialOptimizationLastFrame()
  • LocalInertialBA()
  • MergeInertialBA()
  • InertialOptimization()(4个重载)

我们可以进一步查找每个函数的调用情况,进一步挖掘出调用关系(该函数在哪里被调用了)。 我们可以得到每个函数的调用情况如下。

1.1 FullInertialBA()调用情况

如下图所示。 可以看到,FullInertialBA()函数在两个地方被调用:在LocalMapping的InitializeIMU()函数中被调用,在LoopClosing的RunGlobalBundleAdjustment()函数中被调用。

1.2 PoseInertialOptimizationLastKeyFrame()调用情况

如下图所示。 可以看到,PoseInertialOptimizationLastKeyFrame()函数只在Tracking的TrackLocalMap()函数中被调用了一次。

1.3 PoseInertialOptimizationLastFrame()调用情况

如下图所示。 可以看到,类似的,PoseInertialOptimizationLastFrame()函数同样是在Tracking的TrackLocalMap()函数中被调用了一次。

1.4 LocalInertialBA()调用情况

如下图所示。 可以看到,LocalInertialBA()在LocalMapping的Run()函数中被调用一次。

1.5 MergeInertialBA()调用情况

如下图所示。 可以看到,MergeInertialBA()分别在LoopClosing的MergeLocal()MergeLocal2()函数中被调用了2次。

1.6 InertialOptimization()调用情况

InertialOptimization()共有4个重载。

第一个重载,如下图所示。 该重载在LocalMapping的InitializeIMU()函数中被调用一次。

第二个重载,如下图所示。 该重载在LocalClosing的MergeLocal2()函数中倍调用一次。

第三个重载,如下图所示。 该重载只进行了申明,没有被调用。

第四个重载,如下图所示。 该重载在LocalMapping的ScaleRefinement()函数中被调用一次。

2.进一步分析

根据上面的初步查找,我们可以提取几个出现频率比较高的函数InitializeIMU()(出现2次),TrackLocalMap()(出现2次),MergeLocal2()(出现两次)。从函数名可以看出,InitializeIMU()主要用于IMU相关的初始化,TrackLocalMap()则是跟踪局部地图(更多介绍可以参考这篇博客),MergeLocal2()则是用于融合局部多地图。因为这里我们不会过多涉及建图相关内容,更侧重于Tracking,所以我们会重点介绍一下InitializeIMU()函数。

2.1 IMU初始化

IMU初始化主要涉及到InitializeIMU()函数,它在LocalMapping中被申明和定义,这一点可能和我们的认知不太一样。因为对IMU的初始化相关工作应该放在Tracking或者System里去做,为什么要和Mapping线程扯上关系。关于这个问题的回答,稍后会给出答案。

首先,我们也可以“暴力搜索”,看看哪些函数调用了InitializeIMU(),结果如下。 可以看到,所有的InitializeIMU()函数都在LocalMapping的Run()函数中被调用了。而之所以有这么多则是因为有不同的if分支,需要传入不同的参数,例如下面。 InitializeIMU()函数中,涉及IMU的优化有两个函数,也即上面提到的InertialOptimization()FullInertialBA()。根据ORB-SLAM3的论文,在IMU初始化阶段会进行三类优化:Visual-Only、Inertial-Only和Visual-Inertial联合。回想我们这里InitializeIMU()的调用,它是在LocalMapping的Run()函数中倍调用的。而如果已经运行到LocalMapping了,则至少说明Tracking线程已经完成了基于视觉的定位,这主要涉及TrackReferenceKeyFrame()函数和TrackWithMotionModel()函数。更具体而言,在这两个函数中都会调用PoseOptimization()函数,这个函数是纯视觉的位姿优化。

而也正如这篇博客中提到的:在利用上面两个函数进行单帧Tracking之后,如果建好了一些地图,还会调用TrackLocalMap()函数对估计的位姿进行进一步精化。这里面同样会用到优化。简单来说就是,如果有IMU,并且IMU初始化、状态一切良好的话,就在TrackLocalMap()中分别根据不同情况调用PoseInertialOptimizationLastFrame()或者PoseInertialOptimizationLastKeyFrame()进行带IMU的优化,否则就调用PoseOptimization()进行不带IMU、纯视觉的优化。当然这里还是需要明确的是,这里所有PoseOptimization()函数的优化都是针对位姿的,而不是地图点。如果是针对地图点与位姿一起的优化,在ORB-SLAM框架中一般会叫做LocalBA(),全局地图点和位姿一起的优化叫做GlobalBA()

所以上面说了这么多,可以这样理解:在Tracking的时候,我们首先基于纯视觉(Visual-Only)估计位姿,并调用PoseOptimization()函数进行优化。然后等待条件OK时会调用LocalMapping的Run()函数,启动Mapping线程,如果此时IMU还没有初始化,InitializeIMU()函数就会被调用。在这个函数中,首先会进行纯IMU(Inertial-Only)的位姿优化InertialOptimization(),优化完成以后,最后会调用FullInertialBA()进行一个视觉、IMU联合(Visual-Inertial)的位姿优化。这与论文中介绍的三个阶段是对应的。

2.2 IMU初始化之后

当IMU初始化以后,Tracking中优化的大体流程与上面还是类似的,但是多了一些地图的内容:首先数据传输进来,还是利用TrackReferenceKeyFrame()函数或者TrackWithMotionModel()函数进行纯视觉的Tracking,然后调用PoseOptimization()函数进行纯视觉的位姿优化。如果有地图以后,继续调用TrackLocalMap()对位姿进行进一步优化,如果此时IMU可用且满足一些条件,就调用PoseInertialOptimizationLastFrame()或者PoseInertialOptimizationLastKeyFrame()进行带IMU的优化;否则的话,还是纯视觉的PoseOptimization()函数优化。这样Tracking线程中的相关工作就完成了。此时另一边,Mapping线程进行建图。初步建图以后,如果IMU状态OK,就调用LocalInertialBA()进行局部优化,否则调用LocalBundleAdjustment()进行纯视觉优化。最后,如果有回环或满足一定条件,则进行全局优化。IMU可以的话就调用FullInertialBA()函数,否则就是BundleAdjustment()函数。

回答开头的问题,就是InitializeIMU()函数会修改地图内容,更具体说是地图的尺度。这便是ORB-SLAM3中和IMU优化相关函数的简单介绍。

3.系统的RECENTLY_LOST状态

3.1 在哪里被调用

在ORB-SLAM3中,相比于ORB-SLAM2新增了RECENTLY_LOST状态。这个状态是比较特别的,也是引入IMU之后才有的状态。从字面意思来看,就是“最近丢失”。简单来说,就是如果视觉在一定时间内丢失了,那么就当前系统状态就设为RECENTLY_LOST,然后利用IMU相关数据进行位姿的推导。如果超过了一定的时间视觉信息还没回来,状态就被设为LOST。和上面类似的,我们还是利用“暴力搜索”的功能大致先看看在哪里调用了RECENTLY_LOST

首先,这个状态在Tracking.h中被定义,是一个public的枚举类型,如下所示。 然后我们再查找引用,如下图所示。 可以看到,它主要是在LocalMapping.ccTracking.cc两个函数中被调用,其中最复杂的就是在Track()函数中的调用。如果简单归纳,它被调用无非是两种情况,一种是将当前的系统状态设为RECENTLY_LOST,另一种是用在判断语句里,来判断当前系统状态是否为RECENTLY_LOST。在这一部分,我们重点关注前者,也就是对系统状态的赋值。在上图中我们可以看到,RECENTLY_LOST在等号右边的情况只有在Track()函数中,也即下图中选中的两行。

第一处调用

第一处被调用的地方如下图所示。 可以看到它紧接在Tracking的后面,也就是调用TrackReferenceKeyFrame()函数或TrackWithMotionModel()函数之后。系统会把跟踪是否成功的状态赋给bOK。如果不OK的话,系统会进行一些判断,从而决定当前的状态设置为LOST还是RECENTLY_LOST。当然,可以看到,上面一个比较简单的判断就是,只有地图中的关键帧个数大于10的时候,系统状态才有可能被设为RECENTLY_LOST

第二处调用

第二处倍调用的地方如下图所示。 可以看到是在TrackLocalMap()之后,跟踪的状态会返回给bOK。如果说bOK正常,就把当前系统状态设为OK。如果当前系统状态OK,并且有IMU、IMU已经初始化了,那么会重置当前地图,并且系统状态设为RECENTLY_LOST。而如果没有IMU,状态就设为LOST

3.2 RECENTLY_LOST后怎么办

在系统状态被设置为RECENTLY_LOST以后,我们就会在各种判断条件里判断,并根据情况给出下一步操作。如下图所示。 这里举一个Track()函数中的例子,如下图所示。 如果当前系统状态为RECENTLY_LOST的话,系统会进一步判断是否有IMU以及IMU是否可用。核心思路就是现在已经是RECENTLY_LOST状态,在LOST的边缘了,如果有IMU就再“挣扎”一下,否则直接变成LOST。 所以如果IMU状态OK的话就调用PredictStateIMU()函数进行纯IMU位姿估计,否则的话就把状态变量bOK设为False。并且如果当前帧距离丢失帧的时间间隔大于time_recently_lost阈值,也直接设为LOST。在ORB-SLAM3中,这个值被设为5秒。 而如果没有IMU,则直接调用视觉重定位函数Relocalization()。如果重定位成功了,状态就还是RECENTLY_LOST,否则就设为LOST。这整套逻辑是很顺的,也值得仔细推敲。

4.参考资料

  • [1] https://blog.csdn.net/weixin_41394379/article/details/88531446
  • [2] https://blog.csdn.net/weixin_46363611/article/details/113525125

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

返回顶部