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