ORB_SLAM3：IMU初始化过程梳理以及自己的理解

 LocalMapping线程中IMU初始化:
1、为什么要进行初始化？

      因为无法保证世界坐标系（单目初始化参考关键帧）的Z轴正好与重力方向平行，二者有角度，计算该角度的过程就是IMU初始化的过程。

2、IMU初始化过程中不断优化尺度，在单目相机的视觉SLAM中，尺度指的是场景中真实物体的物理尺寸与它在相机图像中所对应的像素距离之间的比例关系。在视觉SLAM中，尺度是一个非常重要的概念，因为它决定了相机观测到的图像特征点的真实空间位置，从而影响了相机的轨迹估计、三维重建和地图建立等功能。具体来说，在单目相机中，由于相机只有一个视角，无法直接测量场景中物体的真实距离，因此需要通过其他方式来估计物体的尺度。常见的方法包括使用单目视觉SLAM系统的初始化信息、运动模型、IMU数据，或通过人工标定场景中物体的尺寸等方式来获取场景中物体的尺度信息。一旦获取了场景中物体的尺度信息，就可以通过该信息来进行场景的三维重建和运动估计等操作，从而提高单目视觉SLAM系统的精度和鲁棒性。

3、IMU主要的误差源：

      IMU的确定性误差厂家已经标定好，我们应用时得到的加速度和角速度值只包含随机性误差，随机误差需要实时估计。因此随机性误差可以归为以下两类：

            1）高斯白噪声（误差）：测量噪声是AD转换器件引起的外部噪声，波动激烈的测量白噪声；

            2）随机游走（零偏Bias）：随机游走噪声是传感器内部机械、温度等各种物理因素产生的传感器内部误差的综合参数，是变换缓慢的bias。

总bias = 零偏重复性产生的bias值 + bias instability（零偏不稳定性误差，不确定性零偏。）

LocalMapping线程中IMU初始化流程:

      IMU初始化分为三个阶段，第一阶段调用的函数为：InitializeIMU(1e2, 1e10, true); 第二阶段（VIBA1）调用的函数为：InitializeIMU(1.f, 1e5, true); 第三阶段（VIBA2）调用的函数为：InitializeIMU(0.f, 0.f, true);      参数一（priorG）：陀螺仪偏置的信息矩阵系数      参数二（priorA）：加速度计偏置的信息矩阵系数      参数三（bFIBA）：是否进行FullInertialBA；

IMU三个阶段初始化的对比

	第一阶段	第二阶段（VIBA1）	第三阶段（VIBA2）
调用的函数	InitializeIMU(1e2, 1e10, true)	InitializeIMU(1.f, 1e5, true)	InitializeIMU(0.f, 0.f, true)
mRwg初值	计算出来的	单位矩阵	单位矩阵
mScale初值	1.0	1.0	1.0
Optimizer::InertialOptimization(.....）	1、lambda初始值设置为1000，追求求解器的收敛速度。 2、初始化参考关键帧的加速度计偏置和陀螺仪偏置信息矩阵参数不同。	1、lambda初始值设置为1000，追求求解器的收敛速度。 2、初始化参考关键帧的加速度计偏置和陀螺仪偏置信息矩阵参数不同。	1、不设置lambda初始值，追求结果的精度。 2、初始化参考关键帧的加速度计偏置和陀螺仪偏置信息矩阵参数不同。
	优化了Rwg、scale、地图中所有关键帧的速度和偏置（bias）
Optimizer::FullInertialBA(.....）	顶点加入地图中最后一个符合要求的关键帧的陀螺仪和加速度计偏置。	顶点加入地图中最后一个符合要求的关键帧的陀螺仪和加速度计偏置。	顶点加入第一阶段IMU初始化之前关键帧的陀螺仪和加速度计偏置。
	优化了地图中所有关键帧的位姿、地图中所有地图点、完成第一次IMU初始化之前所有关键帧的速度。

1、LocalMapping线程处理完了容器mlNewKeyFrames中的最后一个关键帧，并且闭环检测没有请求停止LocalMapping时才会进行IMU初始化；

2、单目+IMU模式下初始化IMU（三个阶段）的条件：

      1）当前地图中的关键帧数量（mspKeyFrames）≥ 10；

      2）当前关键帧的时间戳 - 初始化参考关键帧时间戳（mFirstTs）≥ 2。

3、满足上面的两个条件后将bInitializing设为true，意为正在做IMU的初始化，该标志用于tracking线程中判断是否添加关键帧。这时还是会检查mlNewKeyFrames容器，如果其不为空,处理该容器的关键帧，包括计算BoW、更新观测、描述子、共视图，插入到地图等，容器lpKF存储地图中的所有关键帧；

4、此时正式进行IMU初始化：

InitializeIMU(1e2, 1e10, true); //参数一（priorG）：陀螺仪偏置的信息矩阵系数  参数二（priorA）：加速度计偏置的信息矩阵系数  参数三（bFIBA）：是否进行FullInertialBA

      1）IMU未进行第一次初始化时（mbImuInitialized = false），主要为了获得重力坐标系到世界坐标系的旋转矩阵的初值（Rwg）；

      2）vpKF容器中只有一帧无mPrevKF，因此have_imu_num = N - 1，have_imu_num的值至少为9。dirG：从参考关键帧到vpKF中最后插入的关键帧之间的速度增量（从IMU坐标系转换到世界坐标系下），其中：，dirG相当于把地图中所有关键帧的转换到世界坐标系下，然后取反加起来，即dirG = V(参考关键帧) - V(地图中最新的关键帧) + ,假设速度变化很小，故，这只是粗略的估计了一个初值，因为后面还会优化，这个值越精确越有助于后面的优化； _vel：当前帧的参考关键帧到当前帧的平均速度。对dirG进行归一化，约等于重力在世界坐标系下的方向，世界坐标系下Z轴的方向为gI(0.0f, 0.0f, -1.0f)，求两个向量之间的夹角（ang），vzg表示旋转向量，最后得出Rwg，更新 mTinit = 当前关键帧时间戳（mlNewKeyFrames中最新的关键帧） - 初始化参考关键帧时间戳（mFirstTs）；

      3）若IMU完成了第一次初始化，mRwg设为单位矩阵，mbg和mba设为当前关键帧的偏置；

      4）将mScale设置为1，然后进行优化：

Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA);

            a.当priorG != 0时，设置一个非线性最小二乘问题求解器的初始正则化参数（Regularization Parameter）的值为1e3；

            注：这个参数是用于控制求解器在解决非线性优化问题时收敛速度和结果精度之间的平衡。正则化参数越高，求解器达到收敛的速度就越快，但是结果的精度可能会变得更差。相反，如果正则化参数越低，结果的精度可能会更高，但收敛速度会变慢，同时求解器可能会发生数值上的不稳定。

            b.遍历地图中的所有关键帧（vpKFs），固定关键帧的位姿，不固定关键帧的速度和初始化参考关键帧IMU的陀螺仪和加速度计的偏置参数；

            c.添加初始化参考关键帧加速度计与陀螺仪偏置的边，不固定重力方向（Rwg）和尺度（scale）。继续遍历vpKFs容器，需要满足它有上一个关键帧，然后将当前遍历的关键帧设置为上一关键帧（mPrevKF）的偏置，检查所有顶点指针是否为空，所有非空则构造8元边，最后进行优化，优化后更新了所有关键帧的偏置；

            d.如果优化后的尺度（mScale） < 0.1，则初始化失败,关键帧中的成员变量bImu表示当前关键帧为IMU完成初始化之前的关键帧。

      5）主要用于将 IMU 数据与图像数据进行融合，并更新当前关键帧和参考关键帧的 IMU 位姿和速度等信息；

mpTracker->UpdateFrameIMU(1.0, vpKF[0]->GetImuBias(), mpCurrentKeyFrame);

      6）将mbImuInitialized设为true，意为IMU已经完成了第一阶段初始化，只有第三阶段的IMU初始化的priorG和priorA为0，在下面的这个全局BA优化上会和前两个阶段初始化有所不同。

Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, mpCurrentKeyFrame->mnId, NULL, true, priorG, priorA);

            a.容器vpKFs（地图）中的所有关键帧位姿均不固定，IMU第一次初始化完成之前的所有关键帧速度添加为顶点且不固定。IMU第三阶段初始化时，顶点添加IMU第一阶段初始化完成前的关键帧IMU的陀螺仪和加速度计的偏置且不固定；

            b.添加关于imu的边，要求当前关键帧和其前面的那个关键帧都需要是IMU第一次初始化完成之前的关键帧，都会设置一个6元边。在IMU第三阶段初始化时，还要优化两帧之间偏置的误差（一元边），而在IMU第一阶段和第二阶段初始化时，优化初始化参考关键帧的偏置的误差（一元边）；

            c.遍历地图中所有的地图点，顶点设置为这些地图点且不固定，然后遍历能观测到该地图点的关键帧，取出特征点建立地图点与关键帧之间的边；

            d.更新了地图中所有的关键帧位姿的mTcwGBA值，更新了地图中所有的地图点的mPosGBA值，而不是直接更新成员变量，更新了完成第一次IMU初始化之前所有关键帧的速度。

      7）处理新来的关键帧，然后进入while循环，结束的条件是遍历完了地图中的所有关键帧。先是获得初始化参考关键帧的所有子关键帧存入容器sChilds中，遍历容器sChilds，更新每个子关键帧的位姿，因为父关键帧和子关键帧之间的相对位姿比较准，用其来更新子关键帧的位姿，具体操作如下代码：

Sophus::SE3f Tchildc = pChild->GetPose() * Twc;    // Tchildc：父关键帧到子关键帧的变换矩阵
pChild->mTcwGBA = Tchildc * pKF->mTcwGBA;

      8）更新三维点，容器mlNewKeyFrames中的关键帧不再处理，直接删除，最后更新mState为OK、bInitializing设为false，mnMapChange++意为地图改变了；

5、进行VIBA需要mTinit < 50、IMU完成了第一阶段初始化和tracking线程当前跟踪状态为OK。mTinit的更新：

      进行VIBA 1的条件：mbIMU_BA1 = fale 且 5 < mTinit < 50

      进行VIBA 2的条件：mbIMU_BA2 = fale 且 15 < mTinit < 50

      即单目相机初始化完成后，IMU三阶段的初始化需要

      条件：LocalMapping线程已经处理完mlNewKeyFrames中的最后的一个关键帧，并且闭环检测没有请求停止LocalMapping，同时地图中关键帧数目大于2个。

      1）在IMU第一阶段初始化时会更新mTinit，其值设为：Tracking线程中当前帧的上一帧时间戳 - 初始化参考关键帧时间戳（mFirstTs）；

      2）完成IMU第一次初始化后，计算上一关键帧到当前关键帧相机光心的距离 + 上上关键帧到上一关键帧相机光心的距离（dist），如果距离（dist）大于5厘米，记录当前KF和上一KF时间戳的差，累加到mTinit。

6、单目模式下在地图中关键帧小于等于200时，每10s优化一次IMU的尺度和偏置：ScaleRefinement();

      1）将待优化变量mRwg的初值设为单位矩阵，mScale的初值设为1，调用下面的这个函数进行优化：

Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale);

            a.添加定点：地图中所有关键帧的位姿和速度（固定），优化Rwg和scale；

            b.添加边：8元边；

            c.开始优化，最后取优化后的值。

      2）若优化后的mScale < 0.1，则retuen；

      3）fabs(mScale-1.f) > 0.002或是在双目imu情况下更新地图。