【转载】ORB-SLAM3源码阅读笔记7：对IMU异常状态的进一步分析
文章目录
-
- 1.IMU的使用
-
- 1.1 数据的传入
- 1.2 数据的使用
- 2.对于IMU数据输入状态
-
- Error 2.1: Not IMU data in mlQueueImuData
- 3.对于IMU数据正确状态
-
- Error 3.1: not IMU meas
- Error 3.2: not enough acceleration
- Error 3.3: Not enough motion for initializing. Reseting…
- 4.IMU异常状态小结
- 5.异常状态的结果
-
- 5.1 mbBadImu
- 5.2 mlQueueImuData.size()
- 5.3 mbImuInitialized
- 6.异常状态及后续操作总结
在之前的 这篇博客中，我们对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可能出现的异常状态以及其对应的变量、调用函数和后续步骤。
