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:

原文地址:https://www.cnblogs.com/yueyangtze/p/14144588.html