贡献： * 考虑IMU噪声的概率模型的情况下，将视觉-惯性初始化问题表述为只考虑惯性的最优估计问题。 * 一次性求解了所有的惯性参数，避免了解耦估计所产生的不一致性。这使得所有的估计都一致。 * 不做任何初始速度和姿态的假设，这使得我们的方法适用于任何初始情况。 * 不假设IMU偏差为零，相反，我们将它们的已知信息编码为被我们的MAP估计所利用的概率先验。

步骤： * Vision-only MAP estimation：使用BA初始化并运行短时间(通常是2秒)的单目ORB-SLAM，以获得一个纯视觉MAP估算的 up-to-scale。同时，计算关键帧间的IMU预积分及其协方差。 * Inertial-only MAP estimation：仅针对惯性的优化，使IMU轨迹与ORB-SLAM轨迹对齐，找到尺度，关键帧的速度、重力方向和IMU偏差 biases。 * Visual-inertial MAP estimation：将上一步的解作为完整VI-BA的种子，得到联合最优解。

Vision-only MAP estimation

初始化纯单目SLAM，使用与ORB-SLAM相同的程序来寻找初始运动。在两个初始帧之间使用ORB描述符进行快速点匹配，建立了基本矩阵和单应性模型并进行了评分，得分较高的一种用于寻找初始运动并对特征进行三角化处理。

与ORB-SLAM的唯一区别是，我们以更高的频率强制关键帧插入(4 Hz ~ 10 Hz)，因为积分时间很短，IMU在关键帧之间的预积分具有较低的不确定性。在这段时间之后，我们会有一个由10个关键帧和数百个点组成的缺少尺度的地图，它们都已经经过BA优化。

Inertial-only MAP Estimation

这一步骤的目标是利用视觉获得的缺少尺度的轨迹，从最大后验估计的意义上获得惯性参数的最优估计。因为我们没有一个很好的关于惯性参数的估计，在这时使用full BA 将会过于昂贵，同时容易陷入局部最小值。一个中间的解决方案是边缘化的点，以获得一个先验的轨迹和它的(全稠密)协方差矩阵，并在优化IMU参数时使用它。我们选择一个更有效的解决方案，考虑轨迹为固定的，并执行惯性优化。需要找到的惯性参数为:

\[ \mathcal{X}_k=\{s,\mathbf{R}_{\mathrm{w}g},\mathbf{b},\mathbf{\bar{v}}_{0:k}\} \]

其中： \(s\in\mathbb{R}^{+}\): 视觉的尺度因子。 \(\mathbf{R}_{\mathrm{w}g}\in\mathrm{SO}(3)\): 为重力方向，由两个角度参数化(pitch和roll)，因此重力在世界参照系中表示为\(\textbf{g}=\textbf{R}_{\mathrm{w}g}\textbf{g}_{\mathrm{I}}, \textbf{g}_{\mathrm{I}}=(0,0,G)^{\mathrm{T}}\)是重力的大小, \(\textbf{b}=(\textbf{b}^{a},\textbf{b}^{g})~\in~\mathbb{R}^{6}\)是加速度计和陀螺仪的bias, 由于初始化时间只有1-2秒，随机游走几乎没有影响。所以假设所有关键帧的bias都是常数。 \({\bar{\mathbf{v}}}_{0:k}\in\mathbb{R}^{3}\): 是缺少尺度的从第一帧到上一帧的body速度, 真实的速度\(\mathbf{v}_{i}=s\bar{\mathbf{v}}_{i}\).

误差函数建模为：

\[ \mathcal{X}_k^*=\arg\min_{\mathcal{X}_k}\left(\|\mathbf{r}_p\|_{\Sigma_p}^2+\sum_{i=1}^k\|\mathbf{r}_{\mathcal{I}_{i-1,i}}\|_{\Sigma_{\mathcal{I}_{i-1,i}}}^2\right) \]

其中\(\mathbf{r}_p\)为先验误差，\(\mathbf{r}_{\mathcal{I}_{i-1,i}}\)为预积分误差。定义为：

\[ \begin{align} \mathbf{r}_{\mathcal{I}_{i,j}} &=\left[\mathbf{r}_{\Delta\mathbf{R}_{ij}},\mathbf{r}_{\Delta\mathbf{v}_{ij}},\mathbf{r}_{\Delta\mathbf{P}_{ij}}\right]\\ \mathbf{r}_{\Delta\mathbf{R}_{i j}} &=\mathrm{Log}\left(\Delta\mathbf{R}_{i j}(\mathbf{b}^g)^{\mathrm{T}}\mathbf{R}_i^{\mathrm{T}}\mathbf{R}_j\right) \\ \mathbf{r}_{\Delta\mathbf{v}_{ij}}&=\mathbf{R}_i^{\mathrm T}\left(s\bar{\mathbf{v}}_j-s\bar{\mathbf{v}}_i-\mathbf{R}_{\mathrm{w}g}\mathbf{g_I}\Delta t_{ij}\right)-\Delta\mathbf{v}_{ij}(\mathbf{b}^g,\mathbf{b}^a) \\ \mathbf{r}_{\Delta\mathbf{p}_{ij}}&=\mathbf{R}_{i}^{\mathrm{T}}\left(s\bar{\mathbf{p}}_{j}-s\bar{\mathbf{p}}_{i}-s\bar{\mathbf{v}}_{i}\Delta t_{ij}-\frac{1}{2}\mathbf{R}_{\mathbf{w}g}\mathbf{g}_{I}\Delta t_{ij}^{2}\right) -\Delta\mathbf{p}_{ij}(\mathbf{b}^{q},\mathbf{b}^{a}) \end{align} \]

Visual-Inertial MAP Estimation

---

Reference

[1] Inertial-Only Optimization for Visual-Inertial Initialization
[2] On-Manifold Preintegration for Real-Time Visual-Inertial Odometry
[3] ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM