在之前的这篇、这篇和这篇博客中，我们从各个角度分析了ORB-SLAM3中和IMU相关的内容。这篇博客我们从另一个角度和问题出发，来“审视”ORB-SLAM3。这个问题就是：ORB-SLAM3到底是如何利用IMU辅助进行位姿估计与建图的？注意，这里我们不会涉及IMU本身的一些函数以及对它们的分析(这在之前的博客中基本都已经介绍过了)，比如IMU预积分(PreintegrateIMU())、IMU位姿估计(PredictStateIMU())、IMU初始化(InitializeIMU())等，而是关注IMU在Localization和Mapping中的作用。
要回答这个问题，其实我们需要从系统的大框架上说起。与ORB-SLAM2类似的，ORB-SLAM3也是由Tracking、LocalMapping、LoopClosing这三个主要的线程组成。当然另一方面，我们也可以从前端和后端的角度进行划分。但这里，我们还是按照前者来介绍，尝试回答这个问题。
1.Tracking中的IMU
如前面这篇博客分析的，在ORB-SLAM3系统的Tracking部分，其实主要可以分为两个层次、三个函数。这两个层次分别是帧间Tracking和局部地图Tracking，帧间Tracking对应TrackWithMotionModel()和TrackReferenceKeyFrame()函数，而局部地图Tracking对应TrackLocalMap()函数。所以进一步，我们可以分别研究这些函数中IMU是如何被利用的。
在继续往下之前，我们其实还可以回答一个问题，那就是ORB-SLAM3中视觉和IMU是紧耦合还是松耦合的？什么是紧耦合和松耦合呢？简单来说就是如果视觉和IMU各自独立算出位姿，我们对两个位姿进行融合，这种方式叫松耦合。在这种方式中，视觉和IMU各算各的，彼此不会有干扰。而紧耦合则是相反。在计算位姿的时候我们就会同时利用视觉和IMU的信息，最后只输出一个由视觉和IMU共同计算得到的位姿。明确了紧耦合、松耦合的定义以后，再来看本段开头的问题就很容易了——ORB-SLAM3采用的是紧耦合的策略。因为视觉和IMU的观测都被作为了约束边或者节点用于构建优化图，进而估计位姿。
根据这篇博客的分析，ORB-SLAM中任何一个Tracking第一次(第一帧)都是先进行初始化，然后第二次(第二帧)依次进TrackReferenceKeyFrame()函数、TrackLocalMap()函数，第三次(第三帧)依次进TrackWithMotionModel()、TrackLocalMap()函数，然后往后重复第三次的流程。所以我们下面的介绍也主要围绕这四个函数，看看哪些地方有IMU的身影。
1.1 单目与双目初始化
这里的初始化包括单目初始化(MonocularInitialization())和双目初始化(StereoInitialization())两个函数。
1.1.1 MonocularInitialization()
如下是单目初始化函数。
事实上，对于单目初始化而言，IMU并没有什么实际的参与。唯一相关的就是在某个分支中判断当前帧是否有预积分，如果有的话就删掉并新建一个赋给当前帧，如下。
1.1.2 StereoInitialization()
如下是双目初始化函数。
相比于单目初始化，双目初始化中IMU有一些实际的参与。主要包含两部分。
第一部分是对于IMU数据和状态的判断。在ORB-SLAM3中认为IMU的数据与状态会影响到双目初始化的结果(第二部分会解释为什么)，所以有一些判断，如下。
图中橙色框框出的就是对于IMU数据以及相关状态的判断，如果不满足条件，就退出本次双目初始化函数。而蓝色部分则是和单目初始化函数是相同的。判断当前帧是否有预积分，如果有的话就删掉并新建一个赋给当前帧。
第二部分则是利用IMU的数据设置初始帧的位姿，如下。
这是IMU在系统中第一次发挥作用的地方。系统会根据传入的IMU外参(相机到IMU的变换关系)计算并设置当前帧位姿。当然如果没有使用IMU，那么当前帧位姿就被设为一个4×4的单位阵。到这里也就可以回答第一部分提出的问题了。为什么IMU的好坏会影响双目初始化。因为我们后面会利用IMU的数据设置初始帧位姿。
至此，初始化部分与IMU相关的内容就介绍完了。
1.2 TrackReferenceKeyFrame()
这个函数的作用是估计参考关键帧和当前帧之间的位姿。函数主体部分和ORB-SLAM2中的TrackReferenceKeyFrame()是一样的。在这个过程中，都没有IMU的参与。唯一一处IMU发挥作用的是在函数的最后，如下。
在ORB-SLAM2中，剩余地图点个数是否大于10是函数返回值为true还是false的条件。但是在这里，如果使用了IMU，则直接返回true，无需再进行个数的判断。而至于为什么要这样设计，目前没有太想明白。
1.3 TrackWithMotionModel()
类似的，这个函数的作用也是进行帧间位姿估计，但基于恒速模型，如下。
根据之前博客的分析，这个函数主要包含：上一帧更新、初始位姿赋值、帧间匹配、位姿优化以及外点剔除这5个步骤。
在函数中，IMU扮演了比较重要的角色。首先是初始位姿赋值步骤，如下。
在拥有了IMU以后，如果IMU状态一切正常，那么我们就直接通过PredictStateIMU()使用IMU来估计当前帧位姿，然后直接退出函数并返回true。而如果没有使用IMU或者IMU状态不好，那就只能老老实实地使用恒速模型估计当前帧位姿初值。
另一个用到IMU的地方和上面提到的TrackReferenceKeyFrame()函数是一样的，就是在函数最后的判断，如下。
只要使用了IMU，就直接返回true，而不再判断筛选以后的地图点个数是否大于10。
1.4 TrackLocalMap()
1.2和1.3部分我们分别介绍了帧间Tracking的两个重要函数TrackReferenceKeyFrame()和TrackWithMotionModel()。在这一部分，我们介绍局部地图Tracking函数。在这个函数中，IMU的作用就更大了，下面分别介绍。
1.4.1 优化部分
首先，相比于ORB-SLAM2中直接调用PoseOptimization()进行优化，由于引入IMU且遵循紧耦合策略，所以会根据当前IMU状态进入不同优化分支，如下所示。
可以看到，如果地图中的IMU没有初始化，那就老老实实用普通的PoseOptimization()函数优化位姿。而如果IMU初始化了，但系统刚刚重定位不久，这时也使用纯视觉PoseOptimization()函数优化位姿。如果这些条件都不满足，那么系统就会根据当前地图的不同状态尝试使用联合视觉+IMU的位姿优化函数PoseInertialOptimizationLastFrame()或PoseInertialOptimizationLastKeyFrame()。这两个函数也是ORB-SLAM3多传感器融合紧耦合策略的最直接的提现。
1.4.2 末尾判断部分
使用IMU以后与ORB-SLAM2另一个不同点就是多了一些分支判断，如下。
可以看到，如果使用了IMU，那么剩余地图点数量只要大于等于15即可认为局部地图Tracking成功，否则需要大于等于30个。
1.5 小结
以上便是对Tracking中的IMU的相关分析。可以看到，在双目初始化、帧间运动模型跟踪、局部地图跟踪中都有所涉及。要么是利用IMU的数据对当前帧位姿赋值(初始化、帧间运动模型跟踪)，要么就是利用IMU和视觉的数据进行紧耦合位姿优化(局部地图跟踪)。这里其实对于IMU的使用还是相对简单的，没有很复杂的整合。
2.LocalMapping中的IMU
LocalMapping则是进行建图。正如之前博客分析的，LocalMapping线程是在System类的构造函数中被初始化和启动的。启动之后，就会一直调用LocalMapping中的Run()函数，如下图所示。
所以在LocalMapping中对于IMU的使用，主要就是以Run()函数为源头。
可以看到整体框架和ORB-SLAM2是一样的，都是上来就来了个while(1)的死循环一直执行。通过这个函数再不断调LocalMapping的其它函数。下面就简单对其中用到IMU的地方进行分析。
2.1 IMU初始化
前面我们说了，要想用好IMU，至少应该包含三个部分：IMU预积分(PreintegrateIMU())、IMU位姿估计(PredictStateIMU())、IMU初始化(InitializeIMU())。这其中前两者是在Tracking类中定义的，而后者是在LocalMapping类中定义的。但显然，IMU初始化是十分重要的，因为IMU有一些参数需要估计。在正式使用IMU之前都需要IMU初始化，所以这是ORB-SLAM3和ORB-SLAM2不一样的地方之一，如下。
在这个函数中，我们会用到InertialOptimization()函数进行纯IMU的优化以及FullInertialBA()进行一次视觉IMU联合的优化。而关于IMU具体怎么初始化，就不在本篇博客的范围了，感兴趣可以参考这篇博客。
2.2 视觉IMU紧耦合局部BA
在ORB-SLAM2中，由于只有视觉信息，所以也没得选，只能用LocalBundleAdjustment()函数进行局部BA，同时调整位姿和地图点。但在ORB-SLAM3中引入了IMU，多了观测，而且依照紧耦合思想，自然就可以构建视觉-IMU联合的优化了，如下图所示。
可以非常清晰地看到，如果带IMU且IMU状态可用，那么就尝试调用LocalInertialBA()实现局部视觉-IMU紧耦合优化，否则就还是老老实实使用纯视觉局部BA。
2.3 地图的尺度精化
由于IMU的引入，其实在一定程度上也算是引入了一种绝对观测。这样的好处就是给地图的估计增加了一种绝对尺度约束。所以我们可以根据IMU观测对地图进行尺度精化，如下图所示。
系统先去判断当前IMU状态，如果满足条件，就再进行IMU初始化。否则的话就直接调用ScaleRefinement()函数进行尺度精化。在这个函数中，我们会用到InertialOptimization()函数进行纯IMU的优化。
2.4 小结
得益于IMU的引入，在LocalMapping中我们可以对地图进行尺度精化，并且可以使用视觉IMU紧耦合的优化来进行局部BA。当然了，IMU的初始化也是在LocalMapping中完成的。这里你可能会有个疑问：IMU初始化函数为什么不放到Tracking里，而要放到LocalMapping里。答案是和地图有关。IMU初始化函数看起来就是对IMU一些参数进行了初始化，但事实上会涉及到利用初始化以后的参数对地图进行更新的过程。如果放到Tracking里去做，可能地图更新起来就没有那么方便。所以作者在设计的时候就把它放到了LocalMapping中。
3.LoopClosing中的IMU
LoopClosing其实是我们之前较少关注的一块，之后有时间会单独写一篇来介绍回环与地图融合相关内容。这里就简单介绍。与Tracking、LocalMapping类似的，LoopClosing也是在System的构造函数中被初始化和启动的，如下。
启动了之后与LocalMapping一样，也是一直运行Run()函数，如下。
在while循环里，首先会判断是否需要对新的关键帧做回环检测，如果确定要做的话，就会尝试将新关键帧的内容和之前的内容进行比较。因为ORB-SLAM3中存在多地图融合和回环两个情形，所以分别判断。如果检测到了地图融合条件，就进行地图融合；如果检测到了回环条件就进行回环。两者并不冲突。在逻辑上有个先后顺序。如果两者都满足，先融合地图，再回环校正。最后，再进行一些收尾工作，完成回环检测及多地图融合，开启下一次的循环。
事实上，ORB-SLAM3中的回环和多地图检测主要还是靠视觉，看是否有满足条件的共视区域(利用函数DetectCommonRegionsFromBoW())。在这个过程中IMU并没有什么直接的参与，在检测到回环或者地图融合以后，会有一些作用。下面分别介绍。
3.1 多地图融合
如果说检测到了地图融合的可能性，那么就会进入地图融合分支，如下。
首先，如果使用了IMU，就会判断IMU是不是已经初始化了，如果没有初始化，直接退出后续地图融合步骤，如下。
如果IMU初始化了，就继续进行。在融合的实际步骤中，IMU主要在下图中红色框框出的三处发挥了作用。
既然要融合两个地图，不可避免就要计算这两个地图之间的一个相似变换，在LoopClosing中是用变量mSold_new来表达的。可以看到，如果当前地图没有使用过IMU，那么mSold_new就直接由蓝色框部分计算(第一处)。如果使用了，就尝试利用IMU辅助。具体如何辅助呢？如果使用了IMU，并且IMU状态良好，就只考虑yaw方向上的差异，其它方向都认为没有问题，按照这个思路就可以得到新的mSold_new，这也就是图中绿色框部分的内容(第二处)。最后一处就是，如果使用了IMU，我们就采用MergeLocal2()进行地图融合，否则采用MergeLocal()。这两个函数差异也很容易理解，MergeLocal2()针对有IMU的情况，在函数中调用MergeInertialBA()进行优化，而MergeLocal()针对无IMU的情况，就稍微复杂一些，在函数中会判断是否使用了IMU，如果是，调用MergeInertialBA()进行优化，如果不是使用LocalBundleAdjustment()。不仅如此，还调用了OptimizeEssentialGraph()、RunGlobalBundleAdjustment()进行进一步优化。当然，在RunGlobalBundleAdjustment()中也根据IMU的状态进入了不同的优化分支，这在后面再说。
3.2 回环融合
如下所示，是回环融合的步骤。
可以看到，主要是在两个部分发挥了作用。可以看到是和前面的地图融合类似的。如果使用了IMU，那么就尝试使用IMU作为约束，认为只在yaw方向上有误差，然后调用CorrectLoop()进行回环校正。如果没有使用IMU，就直接调用CorrectLoop()进行校正。
进一步我们可以稍微深入CorrectLoop()函数中看看，如下。
可以看到里面有这么一段。会调用RunGlobalBundleAdjustment()函数进行全局优化。我们再跳到这个函数看下。
可以看到，这里面有个重要的和IMU有关的分支。如果当前地图的IMU初始化了，那么我们就调用FullInertialBA()进行全局优化，否则调用GlobalBundleAdjustment()进行全局优化。
需要注意的是，RunGlobalBundleAdjustment()函数是在一个新的优化线程中执行的，这个线程在LoopClosing中被新建和启动。
3.3 小结
可以看到，在LoopClosing中主要有两方面的作用：第一就是对回环构成一定约束。如果使用了IMU就可以根据IMU的一些观测为变换初值提供一些参考。第二个就是构造不同的优化图，调用不同的函数。如果存在IMU，遵循紧耦合策略，会调用IMU相关的优化函数，否则就只能老老实实使用纯视觉的优化函数。
4.隐秘的优化
在上面三个方面的分析中，我们或多或少都涉及了IMU相关的优化函数。由于有IMU的存在，在函数中需要优化的地方一般都多了个判断，如果IMU状态OK，就调用IMU相关优化函数，否则就是普通纯视觉优化函数。对IMU相关的优化函数，感兴趣的话可以参考之前的这篇博客。
另外你会发现，优化其实并没有专属线程从头到尾负责，随系统启动和结束。而是穿插在Tracking、LocalMapping、LoopClosing三个一直都在运行的线程中，在需要的时候就被调用。一些简单的优化，比如PoseOptimization()(在Tracking的TrackWithMotionModel()函数中被调用), InertialOptimization()(在LocalMapping的InitializeIMU()函数中被调用), LocalInertialBA()甚至是FullInertialBA()(在LocalMapping的InitializeIMU()函数中被调用)都遵循着哪个线程调用哪个线程负责优化的原则，不会专门为其新开线程。一些相对耗时的优化，则会临时新开一个线程，比如LoopClosing的RunGlobalBundleAdjustment()函数，就会临时开启一个mpThreadGBA线程开始全局BA优化。
本文作者原创,未经许可不得转载,谢谢配合
