ORB-SLAM3：单目+IMU初始化流程梳理

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

理论基础

1. 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的估计。
2. Scale converges much faster when it is explicitly represented as an optimization variable, instead of using the implicit representation of BA.
   显式地将尺度表示为一个优化的变量， 而非隐含在BA的表达式中，尺度会更快地收敛。
3. Ignoring sensor uncertainties during IMU initialization produces large unpredictable errors.
   在IMU初始化阶段，忽略传感器的不确定度将会产生更多不可预知错误。

初始化主要作用是估计IMU的bias（上述传感器不确定度）， 恢复单目地图的尺度， 对齐到惯性系。
整个过程分为以下步骤：

1. Vision-only MAP Estimation，纯视觉MAP估计；
2. Inertial-only MAP Estimation，纯惯导MAP估计；
3. Visual-Inertial MAP Estimation，视觉惯导联合MAP估计；
4. 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()函数，见上图蓝色框。

相关解读文章

ORB-SLAM3论文阅读笔记
单目初始化解读（上）
单目初始化解读（下）
单目初始化总结
IMU初始化代码解读