ORB-SLAM3源码阅读笔记7:对IMU异常状态的进一步分析

Nov 18,2021   4576 words   17 min

Tags: SLAM

在之前的这篇博客中,我们对ORB-SLAM3中IMU初始化状态进行了详细的分析。本篇博客则对IMU的其它异常状态进行分析。

事实上,除了IMU未初始化,还有可能出现IMU没有数据,或者IMU数据错误/不可用(比如静止了很长一段时间等)的情况。所以可以简单归纳为三个方面:

  • IMU的初始化状态
  • IMU数据输入状态
  • IMU数据正确状态

对于初始化状态前面已经说过了,这里就不说了,参考上一篇博客就好。本篇博客主要介绍后两个状态。

1.IMU的使用

在进一步介绍之前,还是先简单介绍一下IMU数据的使用流程。

1.1 数据的传入

如果你比较了解的话可以知道,在ORB-SLAM3中,各个example所在的Main()函数文件中都会有两个函数:LoadImage()LoadIMU(),分别用于将所有影像和IMU数据先加载进来。当然具体而言,对于影像,保存的是每个影像的路径,并没有真正加载影像内容;对于IMU,则是加载了每个观测记录,如下图所示。 当加载好这些内容以后,我们就会通过for循环,依次获得某个时刻的观测数据(影像+IMU),并将其传给系统。这里稍稍需要注意的一点是,影像作为一种瞬时数据是没问题的。但IMU我们需要的是两帧之间的所有观测,所以传入的是一个vector。具体做法就是根据影像的时间戳从刚刚加载的所有IMU观测中找到对应的某一段数据,构建一个临时的小vector,传给系统,如下所示。 简单解释一下。这里seq是一个和多影像序列有关的迭代变量,为了简化,我们可以假设只有一个序列,seq都为0。ni是一个迭代变量,用于迭代每个序列中的各个影像。整个while的条件翻译一下就是:把当前帧时间戳之前的IMU观测都读取进来。当然这里会有个问题,我们还需要确定IMU观测的开始时间,不然会递增式地读取数据。而first_imu就是做这个事情的,其可以理解为是一个动态索引,指示了IMU观测的开始时间在vector中的索引。比如一开始,first_imu就为0,所以他比较的就是vTimestampsImu[0]<=vTimestampsCam[1]。因为在外面有个ni>0的判断,所以第一帧不会获取IMU观测。此时,IMU时间戳就是小于当前帧(第二帧)的,所以就会把观测放到vImuMeas这个vector中来,直到条件不满足。并且把first_imu变量设为对应的索引,这样下次就会从本次结束的地方开始。所以ORB-SLAM3中影像帧和IMU数据的对齐方式是:取当前帧和上一帧之间的IMU观测数据,作为当前帧的IMU观测。

准备好视觉和IMU数据以后,作为Tracking前端,最核心的入口是System类的各种TrackXXX()函数,如TrackMonocular()TrackStereo()等。当系统运行起来以后,这些函数就会直接地将影像和某段IMU观测数据传递给Tracking类。具体而言,在这些函数里面,通过Tracking类提供的GrabImuData()函数,将IMU数据赋给Tracking类的mlQueueImuData。之后在Tracking类的各个函数进行位姿估计的时候,用的都是这里传入的IMU数据。 到这里,将视觉和IMU数据传入系统的工作基本就完成了。

1.2 数据的使用

前面说了,系统已经接收到了观测数据,然后各种TrackXXX()函数中就会调用Tracking类的GrabImageXXX()函数进行实际的处理,如GrabImageStereo()。当然GrabImageStereo()也只是“外包”函数。其主要作用是将传入的数据构造成系统能够识别的各种Frame对象,并且调用Track()进行真正的Tracking,如下所示。 对于双目而言,在Frame的构造函数里分别在左右影像提取ORB特征点,然后计算双目匹配点对等一些操作,对于单目,则直接提取ORB特征点,没有计算双目匹配的操作了。然后,系统就会进入真正的核心——Track()函数。

2.对于IMU数据输入状态

简单来说就是输入系统的IMU数据在形式上是否有问题,是否没有数据等。也可以看到,在数据的传入阶段,还没有对IMU数据的好坏进行判断。事实上,这里有一些潜在的风险,比如在查找某段时间内的IMU观测时,如果恰好那段时间IMU没有观测,得到的vector就为空,这样最后Tracking类的mlQueueImuData也就会为空。ORB-SLAM3作为一个成熟的系统,不可能允许这样bug的存在。

Error 2.1: Not IMU data in mlQueueImuData!!

对于这种形式上的数据错误,ORB-SLAM3在PreintegrateIMU()函数中进行了判断,如下。 如果当前帧对应的IMU观测为空的话,直接把当前帧设为已完成预积分,并返回。

3.对于IMU数据正确状态

简单来说就是,IMU的数据已经“平安”地传入了系统,可是这些数据是否正确、可用仍是一个未知数。所以还需要进行一些判断。

Error 3.1: not IMU meas

在预积分时,对于每一帧的预积分结果,函数都会建个临时变量pImuPreintegratedFromLastFrame,并且将其赋给当前帧(Frame类)的成员变量mpImuPreintegratedFrame。另一个预积分相关变量是Frame::mpImuPreintegrated,而Tracking中改变它的值的变量是Tracking::mpImuPreintegratedFromLastKF。这个变量最早在Tracking的ParseIMUParamFile()函数中被初始化,如下。 在创建地图时,它也在CreateMapInAtlats()函数中被再次赋初值,如下所示。 然后,在PreintegrateIMU()函数中赋给Frame::mpImuPreintegrated。 如果当前帧或者上一帧的预积分失败,双目初始化就直接退出。

Error 3.2: not enough acceleration

StereoInitialization()函数中,如果当前帧的平均加速度和上一帧的平均加速度只差小于0.5,就报错返回。

Error 3.3: Not enough motion for initializing. Reseting…

在LocalMapping的Run()函数中,如果运动过小或者时间过短,同样会认为没有足够的运动。 如果这样的话,也把LocalMapping的mbBadImu变量设为True。这个变量也会指示IMU的状态,如下图所示。 可以看到,在Tracking和LocalMapping中都调用了该变量判断IMU的状态。这个变量在LocalMapping构造函数中默认为False,并且在LocalMapping的ResetIfRequested()函数中也设为False。唯一一个设为True的地方就是在Run()函数中。

4.IMU异常状态小结

我们可以简单对上述状态进行总结。对于ORB-SLAM3中的IMU状态,可以分为三个方面:第一个是数据输入层面,判断输入的数据是否为空,对应Tracking::mlQueueImuData.size();第二个是数据正确性层面,判断输入的数据是否正确,对应LocalMapping::mbBadImu;第三个是IMU初始化层面,判断IMU是否已经初始化,对应Map::mbImuInitialized

5.异常状态的结果

另外,我们需要理解的是,在系统判断了这些异常状态以后,都做了些什么(比如设置系统状态为LOST等)。下面也简单进行分析。

5.1 mbBadImu

这里,我们重点关注调用的if语句,如下,可以看到,主要有三处。 第一处调用在Tracking的Track()函数,如下。 如果mbBadImu为True,系统就会调用System::ResetActiveMap()函数重置当前地图,在System::ResetActiveMap()函数中会将System::mbResetActiveMap变量设为True,然后退出Track()函数。

第二处、第三处调用在LocalMapping的Run()函数,如下。 可以看到只要mbBadImu为True(IMU状态不OK),Run()函数的最核心步骤基本就不会执行了,并请求重置相关状态并返回。

5.2 mlQueueImuData.size()

重点关注if语句,主要有两处,如下。 第一处调用在PreintegrateIMU()函数中,如下。 可以看到,如果观测数据列表为空,当前帧直接设为“已经预积分”状态并退出PreintegrateIMU()函数。

第二处同样在PreintegrateIMU()函数中,如下。 可以看到,当mlQueueImuData不为空的时候系统就会根据不同状态删除其中的第一个项目,不断执行,直到为空。

5.3 mbImuInitialized

用于指示IMU初始化是否OK。根据上篇博客的分析,其调用的关系比较复杂。而且也并非直接调用,而是层层“套娃”,最终通过Atlas::isImuInitialized()函数来获得值,如下图所示。 在LocalMapping的InitializeIMU()函数中,如果IMU成功初始化,则Atlas的IMU初始化状态就会设置为True。 在Tracking的CreateNewKeyFrame()函数中,如果IMU成功初始化,则将KeyFrame的bImu变量设为True。 在Tracking的SearchLocalPoints()函数中,根据IMU的初始化状态,为ORB匹配设置不同的阈值。 在Tracking的Track()函数中,根据IMU的初始化状态,进入不同的分支语句。这里可以看到,如果使用了IMU数据,并且IMU已经初始化并优化的话,系统会重新建一个地图,否则就会重置当前的地图。 在Tracking的TrackLocalMap()函数中,根据IMU的初始化状态,进入不同的分支语句进行位姿优化(带IMU或者不带)。 在Tracking的TrackWithMotionModel()函数中,根据IMU的初始化状态,采用不同方法进行位姿估计(使用IMU还是视觉)。 在Tracking的UpdateLocalKeyFrames()函数中,根据IMU的初始化状态,进入不同的分支。

6.异常状态及后续操作总结

最后,如下图所示,我们总结了ORB-SLAM3中IMU可能出现的异常状态以及其对应的变量、调用函数和后续步骤。

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

返回顶部