@
一、主要思想
我们的主要目标是更进一步，在初始化中使用地图估计，适当使用传感器噪声模型。我们的新初始化方法基于以下思想:
- 尽管非线性性质的BA，单目SLAM(或视觉里程计)是足够成熟和稳健的，在结构和运动上获得非常准确的初始解。与唯一的警告，他们的估计是缺少尺度的
- 视觉SLAM轨迹的不确定性比IMU不确定性小得多，在求IMU变量的第一次解时可以忽略。因此，我们采用只有惯性的最大后验估计，以实时的SLAM轨迹为常数。
- 受到[9]工作的启发，我们采用了参数化，明确表示并优化了单目SLAM解决方案的尺度因子
- 不同于[5][7]，我们联合优化所有IMU变量，考虑位置预积分项与线速度和角速度[8]之间的交叉协方差。
二、主要分为以下三步：
- 仅视觉的最大后验估计使用BA在，短时间内(通常为2秒)初始化和运行单目ORB-SLAM[10]，以获得缺少尺度的仅视觉最大后验估计。同时，计算关键帧之间IMU预积及其协方差[8]。
- 仅惯性的最大后验估计仅惯性优化通过对齐imu轨迹和slam的轨迹，去找到尺度因子、关键帧的速度、重力方向和imu的bias
- 仅视觉的最大后验估计将上一步的解作为一个完整VI-BA的种子，得到联合最优解。
初始化之后，我们启动ORB-SLAM VI[5]，执行local VI-BA。我们观察到，在执行完整的VI-BA 5-10秒后，尺度估计精度可以进一步提高，或者，在计算成本低得多的情况下，重复只进行惯性优化
三、详细步骤
3.1 仅视觉的最大后验估计
与ORB-SLAM唯一的区别是，我们以更高的频率(4到10HZ)强制关键帧插入。这样，IMU在关键帧之间的预积分具有较低的不确定性，因为积分时间非常短。经过这段时间，我们得到了一个由10个关键帧和数百个点组成的未知尺度的地图，它是通过ORB-SLAM mapping线程对BA进行优化的。
3.2 仅惯性的最大后验估计
这一步骤的目标是利用视觉获得的缺少尺度的轨迹，从最大后验估计的意义上获得惯性参数的最优估计。因为我们没有一个很好的关于惯性参数的估计，在这时使用full BA 将会过于昂贵，同时容易陷入局部最小值。一个中间的解决方案是边缘化的点，以获得一个先验的轨迹和它的(全稠密)协方差矩阵，并在优化IMU参数时使用它。我们选择一个更有效的解决方案，考虑轨迹为固定的，并执行惯性优化。需要找到的惯性参数为:
\(s \in \mathbb{R}^{+}\) :视觉的尺度因子
\(R_{wg} \in SO(3)\):为重力方向，由两个角度参数化(pitch和roll)，因此重力在世界参照系中表示为\(g = R_{wg}g_I\), \(g_I = (0;0;G)^T\)是重力的大小
\(\mathbf{b}=\left(\mathbf{b}^{a}, \mathbf{b}^{g}\right) \in \mathbb{R}^{6}\) : 是加速度计和陀螺仪的bias
\(\overline{\mathbf{v}}_{0: k} \in \mathbb{R}^{3}\) : 是缺少尺度的从第一帧到上一帧的body速度
-
我们更喜欢使用无尺度的速度 \(\overline {v_i}\)，而不是真实的速度\(v_i = s \overline {v_i}\)，因为这样可以简化初始化过程。
-
由于初始化时间只有1-2秒，随机游走几乎没有影响。所以假设所有关键帧的bias都是常数。
-
值得注意的是，这个公式从一开始就考虑了重力大小，而不像[7]和[5]，需要一个单独的步骤来确定它的值。
我们用\(I_{i,j}\)表示第 i 和第 j 个关键帧之间的惯性测量预积分，用\(I_{0:k}\)表示初始化窗口内连续关键帧之间IMU预积分的集合
\(r_p\) 先验误差
\(r_{I_{{i-1},i}}\) : 预积分误差，预积分误差定义如下：
具体的看Foster的IMU预积分论文
重力方向的更新式定义如下：
尺度因子的更新式定义如下：
bias的估计：当运动导致的信息矩阵小的时候直接当0，足够大了才优化
