Brief
参考文献
ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
Inertial-Only Optimization for Visual-Inertial Initialization
Visual-Inertial Monocular SLAM with Map Reuse
理论基础
- Pure monocular SLAM can provide very accurate initial maps, whose main problem is that scale is unknown. Solving first the vision-only problem will enhance IMU initialization.
纯视觉SLAM可以提供很好的位姿初始化估计，只是缺失了尺度信息，此结果可以用来改善IMU的估计。 - Scale converges much faster when it is explicitly represented as an optimization variable, instead of using the implicit representation of BA.
显式地将尺度表示为一个优化的变量， 而非隐含在BA的表达式中，尺度会更快地收敛。 - Ignoring sensor uncertainties during IMU initialization produces large unpredictable errors.
在IMU初始化阶段，忽略传感器的不确定度将会产生更多不可预知错误。
初始化主要作用是估计IMU的bias（上述传感器不确定度）， 恢复单目地图的尺度， 对齐到惯性系。
整个过程分为以下步骤：
- Vision-only MAP Estimation，纯视觉MAP估计；
- Inertial-only MAP Estimation，纯惯导MAP估计；
- Visual-Inertial MAP Estimation，视觉惯导联合MAP估计；
- To improve the initial estimation, visual-inertial BA is performed 5 and 15 seconds after initialization，在不同时间点重复多次以保证效果。
Steps
Vision-Only
采用ORB-SLAM经典框架纯视觉初始化流程，即进行单目SLAM。
按照关键帧速率4Hz持续运行2s，然后我们可以得到按比例缩放的地图，包括 10个关键帧以及上百个地图点，然后通过Visual-Only BA进行优化。
因子图如下图所示，实际上即为优化特征点重投影误差。
可以得到优化后的轨迹 ，其中上划线表示按比例缩放的变量（即尺度未定）。
Inertial-Only
这一步的目的是获得IMU参数最优估计。
利用前述单目视觉SLAM初始化后稳定运行的数据（包含10个关键帧的地图），以及这些关键帧之间的IMU测量，包括：尺度因子、重力方向、IMU传感器偏置参数（bias，2个传感器各3个轴，共6个参数）、关键帧的无尺度速度。这些IMU测量，放在一起构成状态向量，构建优化问题求解。
这个优化问题的因子图表示为下图，不包含视觉残差，而是多了一个先验残差项 用来约束IMU的bias需要接近于0。
Vision-Inertial BA
一旦视觉以及IMU有了较好的估计后，进行一个VI联合优化进一步对这些参数进行精化，优化因子图如下。
Implementation
纯视觉SLAM初始化实现参考文末单目初始化解读系列文章。
后续步骤流程梳理如下图：
IMU初始化流程主要为3次对InitializeIMU()函数的调用，见上图红色框。
纯视觉初始化后到累积10个关键帧之前，使用纯视觉BA，见上图绿色框。
当视觉-惯导初始化完成后，LocalMapping正常进行VI-SLAM功能时，调用LocalInertialBA()函数，见上图蓝色框。