ORB_SLAM3 IMU初始化

理论

• 假设单目SLAM的轨迹的误差相对于IMU的误差可以忽略，可以将视觉SLAM的轨迹视为常量
• 考虑了IMU的噪声，然后利用一个MAP仅估计惯性参数
• 一次性求解所有惯性参数，避免在解耦过程中造成不一致性。
• 不对初始速度和姿态做任何假设
• 不假设IMU的参数为0，而是将其已知信息作为MAP估计的先验信息

3 Step

Vision-only MAP Estimation

• 主要获取精确的初始化视觉地图，但是尺度未知
• 关键帧插入频率为4HZ，在2s内，地图中关键帧数量大约10帧以及数百个地图点，只做纯视觉BA优化
• 同时计算关键帧之间的IMU预积分与协方差
• 强制固定高频率(4-10HZ)插入关键帧，因为积分时间短，保证关键帧之间的IMU预积分不确定性低
• 将纯视觉关键帧的位姿通过相机与IMU之间的标定外参转换到body坐标系下

Inertial-only MAP Estimation

问题：如果对于IMU参数没有比较好的初值，那么full VI-BA将不能很快收敛，特别耗时，而且容易陷入局部最优
方法：

1. 边缘化点以获得轨迹的先验和协方差矩阵，然后使用其去优化IMU参数
2. 固定轨迹，只进行Inertial-only优化

首先，对齐IMU轨迹与纯视觉轨迹，求解尺度、关键帧的速度、重力方向、IMU的biases
该优化问题：${\mathcal{Y}}_k^{\ast }=\underset{{\mathcal{Y}}_k}{\mathrm{argmin}}\left(\Vert {\mathbf{b}}^a{\Vert }_{{\Sigma }_b^{-1}}^2+{\sum }_{i=1}^k{\Vert {\mathbf{r}}_{{\mathcal{I}}_{i-1,i}}\Vert }_{{\Sigma }_{{\mathcal{I}}_{i-1,i}}^{-1}}^2\right)$ ，包括先验和IMU残差，其中优化变量： y k = { s , R w g , b , v ˉ 0 : k } y_{k} =\left \{ s, R_{wg}, b, \bar{v} _{0:k} \right \} yk​={ s,R