[论文笔记|VIO]Accelerated Visual Inertial Navigation via Fragmented Structure Updates

Ⅰ. 摘要

VI系统的计算瓶颈在于联合优化，因此这篇论文采用分块fragments来解决这个问题。一组来自多个不同相机的共可见特征点被有效地分割、分类，然后使用由频率模式增长算法激励的机器学习进行分析。此外，在预积分过程中使用连续形式，以提高精度，并且只需要较少的计算资源。实验表明推导不仅更精确，而且需要的计算资源显著减少。

贡献

1. 修改了优化方法的增量特性，从而使过程变得更简单——分块fragmentation–并不考虑所有的变量，仅考虑有最多特征点的相机帧，将其归为fragments。通过将问题分解成更小的信息块，系统可以享受稀疏化的几个好处，从而降低了计算成本。它还允许我们使用固定滞后平滑跟踪更多的帧。加上在NEON intrinsics中编码系统的视觉前端，估计器可以快速估计状态，并与ARM处理器上更多的特性一起工作。
2. 在IMU预积分中使用连续线性动力学，使该过程比基于离散的预积分更精确，计算成本更低。通过将连续方法引入到full-BA优化问题中，可以成功计算IMU的状态和偏差，从而提高了系统的定位精度。
3. 采用了ICE-BA中描述的边缘化策略，但仅适用于计算能力更强的平台。在实验部分，我们展示了系统的鲁棒性。

Ⅱ. 分块BA

A. Bundle Adjustment

为了最优地跟踪系统的当前状态，需要最小化一组最近的相机帧的特征重投影和IMU测量的误差，该优化可以描述为对一组路标$L_k$和相对IMU状态 C k = { R k , p k , v k , b k g , b k a } C_k = \{R_k, p_k, v_k, b^g_k, b^a_k\} Ck​={Rk​,pk​,vk​,bkg​,bka​}的极大似然估计问题。提出了一个固定延迟公式，其中优化是在最近历史关键帧(一直到当前时间帧$k$)的集合$K$中完成的。对于固定滞后优化，使用滑动窗口内包含的最近测量值。–>将完整状态 θ k = { C k , L k } θ_k = \{C_k,L_k\} θk​={Ck​,Lk​}表示为从滑动窗口开始到当前时间帧$k$(滑动窗口的最新测量值)的所有状态的集合。最后，我们将一直到当前时间帧收集到的原始IMU和特征测量集合表示为$Z_k$。最佳求解结果${\theta }^{\ast }$如下：

1. 重投影误差：
   对于当前帧$i$，将每个特征度量(路标点)$l_j\in L_i$投影到相机帧中，成为$R^2$中的二维度量；然后与实际匹配图像测量$z_j$进行比较，得到重投影误差:
   
   其中，${\Sigma }_{lj}$是匹配图像测量$l_j$的协方差，$\pi (\cdot )$是映射函数(这里，用逆深度表示)。

2. IMU误差
   IMU状态量的残差用$r_C$表示，协方差为${\Sigma }_C$,注意，$C_i$表示的是两个关键帧$i$和$i+1$之间对应的IMU测量。

3. 先验残差
   第三个残差$r_{C_0}$是第一个协方差状态$C_0$的先验,其协方差为${\Sigma }_{C_0}$。

我们需要同时实现先验和IMU残差的原因是由于在视觉惯性系统中，重力轴周围的位置和角度都是不可观测的。注意，先验和IMU的误差函数将在第三节中详细讨论。公式(1)可简化为下面的固定滞后式：

其中，$||r|{|}_{\Sigma }^2=r^T{\Sigma }^{-1}r$。

B. 增量公式

通过利用一个稀疏信息矩阵$I$来编码一个测量窗口内状态变量的变化，我们不需要在每次相机和IMU测量到达时重构整个历史测量。因此，只在测量值到达时，增量地修改信息矩阵。

在本节中,我们专注于以增量的方式恢复$\theta$的重投影误差$r_l$和协方差${\Sigma }_l$。这可以用高斯-牛顿迭代算法来找到线性最小二乘解。我们从初始估计值${\theta }_0$和初始误差$r_0$开始。在第$i$次迭代中，计算${\theta }_i$邻域内近似解的修正${∆}_{i+1}$:


公式(5)最终是一个线性最小二乘问题，它也预先考虑协方差。

为简便起见，从现在起，${∆}_{i+1}$记为∆。
为了求出∆，我们可以对Hessian信息矩阵$I$使用舒尔补。我们从$C$中边缘化$L$，从而得到如下矩阵块：

剩下的舒尔补操作都是很常规的知识了，，，

剩下的${∆}_L$回带求解。
我们可以通过简单的增量更新$\delta L^{-1}$来有效地求解这些方程，但是对于需要修改的点太多的情况，$\delta L^{-1}$通过非递增的步骤，几乎可以达到同样的密集程度，这违背了递增化简的目的。我们提出了一种增量方法(下文C部分)，它能够增量地解决$\delta L^{-1}$稀疏或者稠密情况下的Schur补。

C. Fragmented Acceleration

为了极大地加速公式(8)的迭代求解，求解器必须能够处理更新的$\delta L^{-1}$稀疏或密集的情况。其他增量式只考虑稀疏情况，因此当滑动窗口中的帧共享相同特征时(因为必须更新所有变量的状态)，也就是稠密状态时，必须求助于昂贵的批量更新。为此，我们通过改变用于优化的结构点集来修改公式(8)的增量更新步骤。

我们可以简单地从系统的密集部分中省略变量$\delta L^{-1}$中的孤立点，以提高效率，并选择与优化器进行额外的迭代，以确保完整性。最终，这种操作不会像以前那样破坏我们的信息矩阵，而且从长远来看会使问题变得更简单。

既然我们设计了一种方法来保持信息矩阵的稀疏性，那么问题就转移到如何使增量方案在保持效率的同时利用大部分信息。

ICE-BA检查观察到公共点的较小相机片段，但是它使用了一种效率较低的蛮力方法，显式地搜索共享最多点的一组相机。它也不太准确，因为它可能不包括共享高数量的点但超出范围的相机。我们可以通过构建FP树(频繁模式树)来确定多相机共享的点和非共享的点。在我们的BA问题中同样地利用这一点，其中相机被表示为观察到大量特征点的节点。

以增量的方式使用我们的FP树，通过添加传入的相机测量值作为新节点，以及删除滑动窗口后面的旧相机节点，我们的FP树的一个例子如图1所示，也称为FP-growth算法。使用FP树，我们可以很容易地找到共享最多共同点的块fragments，并解决Schur补更新步骤。过于孤立的点不被考虑。为了表示点的孤立度，我们可以选择一个点-相机比率(相当于FP树中的支持度)的限制，在这个限制下，这些点不包含在Schur补计算中。

例如，如果我们选择一个4点-相机比率的限制，我们不包括单台摄像机看到的3个点或2个相机看到的4个点。从那以后我们的Schur补码计算中没有包括所有的点，误差优化变得非常方便，并且可以在滑动窗口中使用许多帧来获得更稳健的解决方案。

图1.FP树结构的图形表示。$\delta L$只是为了假设，并不代表真实的尺寸。绿色的点是被观察到的特征，红黑的矩形是相机-imu系统。我们可以通过添加(红色操作)相机-特征点关联或修改(粉色操作)已存在的节点，从任意根节点构造FP树。

III. 连续预积分

IMU在其内部陀螺仪和加速度计的噪声影响下输出测量值${\tilde{w}}_k$和${\tilde{a}}_k$，其真实值为${\omega }_k$和$a_k$。因此，标准IMU运动学模型可以描述如下:

其中，^运算符表示斜对称运算，$p_k、v_k$为时间帧$k$处的位置和速度。

给定关键帧k处的状态$x_k$，我们可以使用预积分过程(即将两个连续关键帧之间的IMU测量值组合成一个运动约束)来预测系统的未来状态$x_{k+1}$。对于预积分公式，首先使用公式(9)定义IMU速度和位置的变化如下：

其中，∆R，∆p，∆v是针对连续两帧图像k和k+1之间的偏差进行线性化的预积分测量值，除非另有规定，$∆t=t_{k+1}-t_k$。雅可比矩阵指的是一阶近似，比如：

我们使用一个局部表示法来计算IMU测量时间–帧$\tau$的下一个速度预积分均值:

在加速度估计中考虑了重力的影响，以考虑IMU逆重力方向旋转的情况，因此我们在它自己的坐标系中估计加速度。

我们可以将预积分平均计算简化为连续公式，在IMU测量上依次进行:

其中，$∆t=t_{\tau +1}-t_{\tau }$；${\delta }_t=t_{\tau +1}-u$，u是介于$\tau$和$\tau +1$之间的积分常数。exp是从$R^3$映射到SO(3),我们可以用Rodrigues方程去掉积分符号。总体而言，连续预积分意味着更新步长的动态模型比离散方法更简单。

由于我们定义了描述运动约束所需的一切，完全的误差状态不确定性可以通过利用(10)、(11)、(12)来定义：

$[{\hat{R}}_j,{\hat{v}}_j,{\hat{p}}_j,{\hat{b}}_j]$是时间框架$j$下估计的状态，$r_{C_0}$为先验残差，$r_{C_i}=[r_R,r_p,r_v,r_b{]}^T$ 是IMU误差，$\delta b$是估算偏差和实际偏差之间的差值，$∆t=t_j-t_i$。

为了准确估计预积分测量中的噪声，需要得到$r_C$中的离散协方差${\Sigma }_C$（本节里记为$P_k$），开始时，协方差初始化为$0_{15\times 15}$。我们将$F_k$定义为雅可比变换矩阵，并假设它在两个连续IMU测量值之间变化，$P_k$可以通过一阶泰勒展开计算，并通过预先确定的IMU对角噪声协方差矩阵$Q_0$递增：

Ⅳ. 实验结果

在这项工作中，我们表明，通过分块可以显著加速BA过程。为了进一步提高算法的速度，我们使用连续形式的预积分，并使用NEON内置函数来有效地提取和跟踪图像特征。总的来说，本文算法是两全其美;一方面，它和基于优化的方法一样精确，另一方面，它在计算上和基于滤波的方法一样轻量。