ORB-SLAM笔记——ORB-SLAM3的IMU初始化（1）理论部分

上一篇blog立的flag，哎最近工作虽然也是快忙炸了但面对这么好的一个作品，你就忍不住挤时间也要学他啊哈哈。那么我们开始！

在正式说它的IMU初始化部分之前，还是跟随作者论文的思路，咱们再把Visual-Inertial的架构梳理一下。

首先对于一个纯视觉的系统来说，单目也好，双目也罢，我们主要关心的还是每一个pose的6Dof，对应的状态空间呢也就是六维。考虑到单目的尺度问题，状态空间也就是多了一个scale。但在一个visual-inertial SLAM系统中，我们在对当前pose进行估计的同时，还要同时估计世界系下的速度，以及IMU中，gyro和accelerator分别的bias。这里为了一路从visual SLAM走来的小伙伴们说的碎一点，于是自我提问一句，这是为什么呢？对于以定位和建图为目标的我们来讲，去估计pose和landmark的位置是我们的核心诉求，为啥还要操更多心去每帧多估计九个参数呢？这首先要看，在一个VI SLAM的系统内，我们构建优化问题时，残差方程都包括哪几部分。先放paper中的图：

上图是不同优化问题的因子图表达，对于之前对纯视觉方案接触更多的小伙伴们来说，对（b）应该不陌生，整个目标问题也比较简单，就是优化Tk使得重投影误差达到global minimum。那么对于一个多传感器系统，在运动过程中，周围的landmark对camera的运动形成约束的同时，我们用IMU的数据以及其运动方程同样可以得出一段运动轨迹。因为不同的sensor都附属在一个刚体之上，那么视觉和IMU分别得出的运动是有联系的：通过外参，我们可以去用视觉和IMU各自的约束去共同优化刚体的运动。这也就是（a）中所要表达的，此时的Tk要代表刚体的pose，而不是某一个sensor；红色方块和（b）中一样，还是重投影误差构成的视觉约束；黄方块是新的，即IMU的运动约束；紫色块呢是bias构成的残差，不清楚的小伙伴可以去看一看IMU的建模，了解下bias在时间轴上被建模成了一个怎样的过程。图里画的还是挺清楚的，每一个黄色块，都和相邻前后帧的速度，bias相连接，这说明这些状态变量都参与了黄色残差方程的构建。所以，比如我们要利用IMU约束去优化Ti时，是需要上一帧的v_i-1和bias_i-1来参与计算的。这也便回答了为什么要额外估计这些状态变量了。

对于（a）中的运动残差，作者也给出了上述表达式，其中带三角的R，v，p都是IMU计算出的观测量，这些量和目前正估计的状态值共同形成了约束。这里作者只给了最终形态的公式，建模的具体过程以及残差对于状态空间的雅可比的推导，对这些还不熟悉的小伙伴可以去看Forster关于IMU在流形上的预积分的paper，包括这个work在内的很多VI-SLAM/VIO基本都是用Forster的理论的。读paper的同时也可以参考网上的很多大神的推导，其中推荐Jinge Tu的blog，最早啃那些公式的时候也是从中获益良多。

好了，以上，铺垫完了。咱们进入正片，讲一下orb-slam3中的IMU初始化。

初始化的目的是什么？这是每个VIO/VLSLAM的同学都要有，也都会有过的灵魂拷问了。首先咱们先不直接去想初始化的目的，而是先想想以下几个问题：

1. 我们的系统建立了一个世界坐标系，这个系的重力方向是不是和在惯性系中的方向重合呢？如果不重合，同时又恰好我们的世界系需要和惯性系的重力相重合时，我们肯定得想个办法先把重力方向找出来，然后系统再正常跑起来。

2. 之前提到的运动约束，我们需要上一帧的v和bias参与计算，那么最初的v和bias是怎么来的呢？是不是要在找到重力方向的同时，再想个办法将系统这一段时间内的速度和bias先估计得准确一点。

以上问题，如果我们不想办法了，直接跑，是不是行不通呢？那好，我们来考虑一个初始化过程，把重力方向Rwg，这一初始过程内的速度v0:k，以及bias给估出来。

先直接说orb-slam3是怎么做的，三个过程，三个MAP问题贯穿始终：

（1）纯视觉的最大后验估计：

为什么一开始先跑纯视觉的，作者这里说了，抛开尺度不谈，纯视觉在提供一个良好初值的能力上，还是有的。这个阶段会跑2s，向地图里插十个左右关键帧，landmarkd大概几百个左右，整体是前文图中的b过程，构建一个纯视觉的BA，优化Tb，这些Tb是给第二阶段计算IMU运动残差来用的。

（2）纯IMU的最大后验估计：

这一阶段的目标：估出尺度s，重力方向Rwg，bias，v0:k。为什么bias只估一个呢，这里采用初始化阶段将bias固定不变的方法，将初始阶段的sensor uncertainty考虑为一个微小且相对稳定的过程。此时的目标问题如下：

残差包括由公式（2）组成的IMU运动残差部分，以及一个先验的残差块，这个是与IMU的bias相关的，确保系统刚一开始的bias只是一个微小量。

当这些量被估计出以后，系统会根据尺度s将pose和landmark进行缩放，并根据Rwg将世界系掰正。此时因为这一阶段的所有pose的bias也被同步更新了，所以这一阶段的IMU预积分也会重做。

（3）VI的最大后验估计：

当以上做完之后，作者把这一阶段的所有状态全放到一块儿再整体考虑一遍，这个过程很像（a），是一个visual-inertial optimization。但作者也强调了这个阶段和正常跑VI-SLAM还是有区别的，比如说在这一阶段的所有keyframe对应的bias还都是相同的。但这里尚有一点不理解，including the same prior information than in the inertial-only step，那在这一阶段先验究竟是怎么用的呢？欢迎有想法的朋友们评论区指点下哈，多谢多谢。

那么以上是作者阐述的初始化全过程，我自己还没有跑，不过paper里给出的前两秒5%的scale误差还是相当可以的，同时说在初始化后5s和15s分别做VI-BA，可将scale误差优化到1%。这一点上的话，倒是引起了我一个额外的注意，他的VI-BA在初始化已经结束，正常跑起来后，这么久才做一次吗？关于这可能还是要靠啃源码才能了解作者的思路了。作者在这里对比了一些初始化方案，突出orb-slam3的一大最直接的有点就是快了吧。的确很快，而且记得当时读VI-DSO的paper时，TUM他们好像还有说相比VI-ORB，自己的初始化是很快的哈哈哈。

那么本篇blog到这里就先结束了，这一部分仍只是一个概述性的总结，后续还要从代码层面将这一部分继续深挖下，理清它的初始化是如何贯穿在各个线程中的。