ORB-SLAM3中IMU初始化

发布时间 2023-12-27 14:55:23作者: narjaja

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