ORB_SLAM3 IMU初始化（Inertial-only MAP estimation）

学习ORB_SLAM3时在IMU初始化时，有些疑问故以笔记的形式记录下来：

ORB_SLAM3 IMU初始化是基于论文《Inertial-Only Optimization for Visual-Inertial Initialization》分3步：第一步把冰箱门打开。。。。

1.Vision-only MAP estimation(纯视觉的最大后验估计)

2.Inertial-only MAP estimation(纯惯导的最大后验估计)

3.Visual-inertial MAP estimation(视觉&惯导的最大后验估计)

1.Vision-only MAP estimation(纯视觉的最大后验估计):

初始化纯单目SLAM,在初始的两帧使用FAST提取和ORB描述子做2D-2D匹配，然后使用本质矩阵和单应矩阵模型计算变换矩阵，然后使用评分高的模型进行初始化运动和三角化特征点，初始化成功后以比较高的频率（4 Hz to 10 Hz）插入关键帧，在关键帧之间需要进行IMU的预积分。要进行IMU的外参标定。

2.Inertial-only MAP estimation(纯惯导的最大后验估计)：

仅使用IMU数据，通过轨迹被固定，也就是关键帧的位姿被固定，对IMU的参数进行优化。

关键帧状态，除了位置其他参数定义如下：

 

 放缩尺度 

 重力方向的旋转 

 6轴的零偏：三轴（X，Y，Z）加速度零偏，三轴角（Pitch,Yaw,Roll）速度零偏 

   0~K帧关键帧除去尺度的3轴（X，Y，Z）速度

 

     第0到第k关键帧帧的IMU积分 

  从第0到第k帧的IMU测量值固定，第k关键帧数据分布概率

  第k关键帧数据固定, 从第0到第k帧的可能的IMU测量值分布概率

  第k关键帧数据分布概率 

最大后验估计：

 

第二行，log函数单调性

第三行，最大值=取反最小值

第四行，根据贝叶斯公式：

 

第五行，log展开

第六行，删除无关项

第七行，似然展开

  

如果IMU预积分和先验的满足高斯误差分布，最大后验问题等效于：

   

  先验残差 

帧间残差 

对应代码部分：

Optimizer::InertialOptimization

图优化对应3种类型的边：

EdgePriorAcc =>先验加速度零偏残差

EdgePriorGyro=>先验角速度零偏残差

EdgeInertialGS=>帧间残差

TODO: