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

• 1.暴力查找
  • 1.1 FullInertialBA()调用情况
  • 1.2 PoseInertialOptimizationLastKeyFrame()调用情况
  • 1.3 PoseInertialOptimizationLastFrame()调用情况
  • 1.4 LocalInertialBA()调用情况
  • 1.5 MergeInertialBA()调用情况
  • 1.6 InertialOptimization()调用情况
• 2.进一步分析
  • 2.1 IMU初始化
  • 2.2 IMU初始化之后
• 3.系统的RECENTLY_LOST状态
  • 3.1 在哪里被调用
    • 第一处调用
    • 第二处调用
  • 3.2 RECENTLY_LOST后怎么办
• 4.参考资料

在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.cc和Tracking.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