ORB-SLAM3中IMU初始化

ORB-SLAM3中IMU初始化由LocalMapping线程中的InitializeIMU函数完成。
主要是完成重力方向\(R_{wg}\)和尺度scale的估算，总共进行三次。
InitializeIMU函数包含两部分：InertialOptimization 和 FullInertialBA

InertialOptimization函数

纯IMU的优化，固定关键帧位姿，优化重力方向、尺度、关键帧速度和偏置

FullInertialBA函数

根据上一步得到的\(R_{wg}\)和scale变换到整张地图，再进行IMU+视觉的整体优化
优化的节点：关键帧位姿、速度、加速度和陀螺仪偏置、地图点
优化的边：重投影、旋转变换、位置变化、速度变换、相邻关键帧偏置

第一次初始化

触发条件：10个以上关键帧，关键帧累计时间超过2s(单目)，没有进行过IMU初始化
初值：\(R_{wg}\)通过速度变化近似得出，scale设为1

第二次初始化 VIBA1

触发条件：完成第一次初始化、累计时间>5s
初值：\(R_{wg}\)为单位矩阵，scale设为1

第三次初始化 VIBA2

触发条件：完成第二次初始化、累计时间>15s
初值：\(R_{wg}\)为单位矩阵，scale设为1

优化\(R_{wg}\)和scale

后面100s内每隔一定间隔优化\(R_{wg}\)和scale
ScaleRefinement函数仅调用InertialOptimization