3.3ORB-SLAM3之视觉与IMU的联合初始化(待补充)

总述

先放一张LocalMapping的代码结构图

相比于ORB-SLAM2，ORB-SLAM3的系统初始化分成了三个主要的模块：纯视觉初始化、纯IMU初始化、视觉和IMU联合优化。

纯视觉初始化和之前一样就是单目或者双目初始化，在Tracking线程中进行

纯IMU初始化和视觉与IMU联合优化目的是为了通过视觉创建的关键帧以及关键帧之间的积分和位姿，获取精确的IMU不确定度即偏置信息，减小IMU传感器数据的不稳定性。

参考链接：《ORB-SLAM3：单目+IMU初始化流程梳理》

1.纯视觉优化

纯视觉优化并未作出改动，对于单目模式通过自适应初始化模型计算相邻帧的运动，并通过三角化恢复缺少尺度信息的3D地图点和位姿信息

对于双目和RGB-D模式则可以直接获取地图点的3D坐标

if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD)
    StereoInitialization();
else
    MonocularInitialization();

详细内容见博客《ORBSLAM3之双目初始化和单目初始化》

2.IMU初始化

这一步的目的是获得IMU参数最优估计，对于单目来说还要回复尺度信息并将视觉地图对齐到惯性系。

纯IMU的初始化要进行三个阶段，目的是为了不断的优化IMU传感器的不确定性即IMU偏置、scale等参数，并联合视觉定位信息进行联合优化，进一步优化智能体的位姿和地图点信息。

2.1 IMU初始化代码逻辑

在VI模式下，对于局部地图线程的每一次循环，都会进入IMU第一阶段初始化函数，并且判断当前活跃子地图中关键帧数量(大于10帧)和距离初始关键帧的时间(单目1s双目2s)是否满足IMU初始化的要求，因为对于IMU来说需要足够的运动才能进行准确的初始化。

第一阶段的初始化，只对IMU的偏置bias信息、scale以及相关残差进行优化，不涉及与视觉和IMU的联合优化。这个优化问题的因子图表示为下图，不包含视觉残差，而是多了一个先验残差项 用来约束IMU的bias需要接近于0。

if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial)
{
    if (mbMonocular)
        InitializeIMU(1e2, 1e10, true);
    else
        InitializeIMU(1e2, 1e5, true);
}

同样的，为了获得足够的运动，IMU的第二次初始化需要在第一次初始化5s之后再进行，第三次需要在第二次成功后15s进行。

2.2 IMU初始化InitializeIMU

函数声明：
IMU模式下需要对IMU进行3次初始化，每次初始化使用同一个初始化函数，但陀螺仪偏置和加速计偏置矩阵系数不同

/** 
 * @brief imu初始化
 * @param priorG 陀螺仪偏置的信息矩阵系数，主动设置时一般bInit为true，也就是只优化最后一帧的偏置，这个数会作为计算信息矩阵时使用
 * @param priorA 加速度计偏置的信息矩阵系数
 * @param bFIBA 是否做BA优化，目前都为true
 */
void InitializeIMU(float priorG, float priorA, bool bFIBA);

2.3 尺度和重力方向优化

当地图中的关键帧数量小于200且为单目IMU模式，需要对尺度和重力方向进行一次优化，在200个关键帧范围内每隔10s对尺度和重力方向进行一次优化

if (((mpAtlas->KeyFramesInMap())<=200) && 
        ((mTinit>25.0f && mTinit<25.5f)||(mTinit>35.0f && mTinit<35.5f)||(mTinit>45.0f && mTinit<45.5f)||
        (mTinit>55.0f && mTinit<55.5f)||(mTinit>65.0f && mTinit<65.5f)||(mTinit>75.0f && mTinit<75.5f)))
{
    if (mbMonocular)
        ScaleRefinement();
}

3.视觉和惯导联合优化

第二次初始化和第三次初始化在对IMU参数进行优化后，需要联合视觉信息进行联合BA优化，优化因子图如下

承接上一步纯imu优化，按照之前的结果更新了尺度信息及适应重力方向，所以要结合地图进行一次视觉加imu的全局优化，这次带了MP等信息

if (bFIBA)
{
    if (priorA!=0.f)
        Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, mpCurrentKeyFrame->mnId, NULL, true, priorG, priorA);
    else
        Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, mpCurrentKeyFrame->mnId, NULL, false);
}