总述

先放一张LocalMapping的代码结构图
在这里插入图片描述

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

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

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

参考链接：
单目+IMU初始化流程梳理
ORB-SLAM3的IMU初始化（1）理论部分

1.纯视觉优化

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

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

if(mSensor==System::STEREO || mSensor==System::RGBD || mSensor==System::IMU_STEREO || mSensor==System::IMU_RGBD)
{
    // 双目和RGBD相机时共用一个初始化函数，因为可以直接得到深度信息
    StereoInitialization();
}
else
{
    // 单目初始化
    MonocularInitialization();
}

2.IMU初始化

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

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

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

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

if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial)
{
    // 在函数InitializeIMU里设置IMU成功初始化标志 SetImuInitialized
    // IMU第一次初始化
    // 
    if (mbMonocular)
        InitializeIMU(1e2, 1e10, true);
    else
        InitializeIMU(1e2, 1e5, true);
}

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

第二次初始化，会对IMU数据做一次BA优化

// 当前关键帧所在的地图还未完成VIBA 1
if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1())
{
    // 如果累计时间差大于5s，开始VIBA1（IMU第二阶段初始化）
    // 距离第一次初始化后需要让IMU攒点数据进行第二次的初始化
    if (mTinit>5.0f)
    {
        cout << "start VIBA 1" << endl;
        mpCurrentKeyFrame->GetMap()->SetIniertialBA1();
        if (mbMonocular)
            InitializeIMU(1.f, 1e5, true);
        else
            InitializeIMU(1.f, 1e5, true);

        cout << "end VIBA 1" << endl;
    }
}

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

if (bFIBA)
{
    // 5. 承接上一步纯imu优化，按照之前的结果更新了尺度信息及适应重力方向，所以要结合地图进行一次视觉加imu的全局优化，这次带了MP等信息
    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);
}