VIO回顾：从滤波和优化的视角

https://mp.weixin.qq.com/s/zpZERtWPKljWNAiASBLJxA

根据以上网页自己做的总结：

在机器人社区中，定位与构图问题属于状态估计问题。主流使用的工具可以对给定噪声测量的动态系统进行随机估计，例如扩展卡尔曼滤波器（EKF）或粒子滤波器（PF）。 一些内部传感器可以测量姿势随时间的变化，这种变化关系可以用公式表示为移动平台的数据驱动动力学模型，包括地面移动机器人中的光学编码器和飞行机器人中的IMU，众所周知它们具有累积误差和测量偏差。 一些外部传感器，如相机或激光测距仪，能够提供角度或距离测量。基于三角测量或三边测量方法，这些传感器能够估计位置移动平台的位置和方向。 估计结果的可靠性在很大程度上取决于环境条件，而且结果对测量噪声敏感。 然而，在状态估计问题中，它们可以用测量模型来公式化表示。于是，EKF或PF能够通过融合动态模型中的分布和测量模型中的分布来推得从上一步到当前的估计分布。在大多数情况下，由于被估计状态数量过多，人们会用PF代替EKF，然后估计出高斯分布的均值和协方差。

在计算机视觉领域，由视觉进行运动估计是一个优化问题[5]。通过图像配准的迭代计算求得变化的相机姿态[6,7]。通常，从一幅图像中提取特征并重新投影到另一幅不同视角的图像中（重投影），这样，最小化特征与重投影特征的误差代价，就可以解算出相机相对结构化环境的姿态变化。基于优化的算法主要是基于梯度的，例如牛顿法或高斯-牛顿法。 多数情况下，我们可以利用矩阵的稀疏结构来提升计算效率。 优化的结果仅提供估计状态的值，而估计分布的信息是不可用的。

利用贝叶斯推理框架可以建立基于滤波和基于优化两种方法之间的联系。基于优化的方法，迭代地找到测量的总概率最高的状态，故可被看作为最大似然（ML）的思路。基于滤波的方法，其中平台姿态的先验分布由内部传感器的测量构建，并且似然分布由外部传感器的测量建立，故可以被视为最大后验（MAP）的思路。在非线性动态模型和/或非线性测量模型中，对EKF中的每个单步执行迭代更新，这样的迭代EKF等价于基于优化的方法。基于优化的方法，可以向内部传感器或其他源的测量中添加正则化项或先验项，使其从ML问题变为MAP问题。当前估计取决于当前和以前的测量，便是在线或“可溯”算法。当前估计取决于整个数据集，或者可以说它们不仅取决于当前和先前测量，而且还取决于之后的测量，这样的批处理过程，属于离线或“非可溯”算法。在基于优化的方法中，批处理过程被转为求解线性代数方程组。在基于滤波的方法中，卡尔曼平滑器能够经由前向通道和后向通道找到后高斯分布。

流行的SLAM技术能估计平台姿态和环境结构。需要通过闭环检测的过程来约束由估计算法引起的累积误差。然而，在VIO技术中没有明确地要求闭环检测[8-10]。 因此，VIO中的估计误差将被累积并且不能被约束。由于没有闭环检测，在小尺寸的装置内就可以进行长时间和大规模的定位。因此，研究重心也就转向了如何开发可以减慢误差累积的技术。

基于滤波的方法

EKF框架通常由预测步骤和更新步骤组成。对于基于滤波的VIO方法，惯性传感器测量得到的三轴加速度和转动速度，可用于计算3D刚体运动的数据驱动动力学模型或先验分布，并且可在预测步骤中用来进行运动预测。相机能够提供特征和移动平台之间的角度和距离测量，可用作测量模型或似然分布，并用于在更新步骤中更新预测。

我们假设这样一个移动平台，只配备一个相机和一个IMU，在未知的环境中移动。相机和IMU之间的空间关系是固定的，而且可以用已知的位姿表示。基于EKF的VIO算法的目的是使用惯性测量和对未知环境的视觉观测来提供平台的位置和方向信息。接下来，我们将基于[11]中的工作给出EKF框架的完整描述，包括状态描述，IMU数据驱动动力学和视觉观测。

2.1. IMU数据驱动动力学模型

任意时刻的3D刚体的IMU状态向量可以由16×1向量定义：

其中是单位四元数，用来描述从世界帧{W}到IMU帧{I}的旋转。和是关于{W}的位置和速度，和是3×1向量，分别用来描述影响陀螺仪和加速度计测量的偏差。帧间的空间关系如图1所示。

2.2 误差状态表示与更新

2.3 视觉测量模型与更新

由于IMU数据中存在偏差和噪声，预测步骤的预测结果会随着时间的增加而变得越来越差。视觉传感器的测量恰好能够提供关键信息以限制误差的增加。为了在EKF框架中用这方法，从图像中提取的关键信息要被转换成测量方程。有各种方法来构建测量模型。例如，松耦合方法中，通过图像配准直接获得位置和方向的变化，然后在EKF框架中融合两者估计结果。[13]而所谓的紧耦合方法则提倡使用从图像提取的关键信息。关键信息可以是通过特征检测器从图像中特征提取得到[14]，也可以由具有深度信息（点云）的直接光强度[15,16]与半直接光强度得到。[17]需要分析关键信息和状态变量之间的关系，使其建模为测量方程。

完整的更新过程总结如下：

（a）对于特定采样频率中的视觉数据，通常在原始图像上采用一些图像处理程序来提取关键信息。

（b）计算残差（7）。

（c）计算卡尔曼增益（8）。

（d）通过添加修正来更新状态。

（e）更新误差状态协方差矩阵（9）。

. 基于优化的方法

基于优化的方法主要依赖于特征提取的图像处理技术和图像配准的最优化，而惯性测量被视为先验，可当作正则项或完全忽略。在大多数情况下，基于优化的方法有两个阶段：构图和跟踪。[15]在构图阶段，由各种特征检测器从图像中提取特征，如3D空间中的角点，边缘或其他地标。然后，对于检测到的所有特征，定义两帧图像之间的重投影误差。该误差用作要优化的损失函数，以便找到特征或地标的坐标。[7]在跟踪阶段中，地图中特征和地标的坐标被用于定义两帧图像之间的重投影误差，并且再次应用优化算法以计算出移动平台的位置和方向的变化。将估计问题分为两个阶段的想法可以获得快速的跟踪结果，然而构图处理还是耗时的。[14,15]对于上述分别由3D特征的坐标和移动平台的姿态改变定义的两个重投影误差损失函数，同时优化是可能的。[19]引入关键帧的概念能够边缘化旧的状态而保持一个有界优化窗口，保证实时性。

3.1 特征配准

迭代非线性优化可以通过最小化图像观测区域的重投影误差来找到相机姿态改变和/或特征坐标。通常，地图由多个关键帧中标识的特征组成，即一系列的3D坐标向量表示，其中关键帧中包含着显著的特征。当获得新的图像时，应当判断其是否是关键帧。如果是，则将该新图像中通过图像配准算法计算找到的新特征坐标与当前相机姿态一起添加到地图中。否则，地图不更新。

图像配准中优化的目的是在每个时间步长中找到变换T，即T被认为是相机姿态。用局部最小的形式表示相机姿态更适合用于优化。

3.2 稠密配准

在上述讨论中，使用图像的特征或地标的坐标来定义重投影误差。然而，它需要特征检测过程，由于特征被稀疏地提取，导致图像中的大部分信息被忽略了。特征提取过程通常存在噪声和不鲁棒等糟糕的问题，因此需要有更高水平的鲁棒估计技术。由于所有这些估计步骤都不在原始图像测量（强度）层级，它们会彻底地传播特征提取误差和累积漂移。

另一方面，基于表观和光流的技术是基于图像的，并且直接基于原始图像的测量[21]做最小化误差，即使用光度（亮度或强度）函数，因此被称为直接或稠密方法。稠密方法旨在使用整幅图像进行配准。非线性优化技术用于找到两个场景之间的变换关系。我们越来越清楚，使用稠密的方法用于构图和跟踪可以获得更加完整，准确和鲁棒的结果。文[22,23]中的工作通过使用Kinect RGB-D相机做视觉里程计，通过最小化光度误差实现图像配准。文[24]中的工作包括最小化光度误差和深度误差。

使用RGB-D相机（例如Kinect传感器），像素的深度信息通过最小化每幅图像中的所有数据间的距离测量实现多个扫描的配准，而不是有限数量的特征或地标。这种稠密扫描能够重建环境中的场景表面并跟踪姿态。给定两个连续的稠密深度测量，从不同视角观察到的同一静态场景的表面或3D点云，一个点云映射到另一个上的最佳刚体变换对应了相机的姿态变化。

迭代最近点（ICP）是通过优化刚体变换来匹配扫描的流行算法。[25]ICP工作方式如下：给定两个对应点集：β= {β1，...，βn}和γ= {γ1，...，γn}，它们之间的平移和旋转分别为t和R，t和R可以通过最小化误差平方和得到：

如果正确的对应关系已知，则计算旋转和平移是有闭合解的。然而在大多数情况下，正确的对应关系是未知的，则通常不能一步求解出相应的最佳旋转和平移。通过找到最近点的对应关系，以迭代的方式进行配准。[25]如果初始位置足够接近，ICP算法会收敛。在文[26]中报道了使用ICP进行视觉里程计的一项工作。

稠密配准也可以通过将当前图像与场景模型匹配来进行。KinectFusion方法为稠密配准提供了实时的解决方案，[27]其在GPU中将构图和跟踪线程分离，并用截断符号距离函数构建场景模型。然而，对整个点云执行ICP算法的计算代价是昂贵的，并且在通用PC上不能提供实时解决方案。

对于单目相机，深度信息未知。在构图线程中通过非凸优化算法求解最小化光度误差来估计反向深度图。[15]在跟踪线程中，使用图像配准。在单目相机中也通过贝叶斯滤波器估计深度信息，随后用优化过程平滑得到的深度图[28]。

为了进一步简化计算复杂度并保持稠密方法的准确性，近年来人们提出了一些半稠密或半直接方法。[29,30]半密集深度图覆盖了整个图像区域，也包括重要的梯度信息。使用贝叶斯滤波器来估计反向深度图，同时通过直接最小化稠密光度误差来实现跟踪。如文[30]中所述，半直接或半稠密方法使用数百个小块来增加鲁棒性，并允许忽略块的法向量。

3.3 惯性测量项

在基于滤波的方法中，IMU传感器的测量作为数据驱动3D刚体运动动力学的数据源，并且通过卡尔曼滤波器与相机的测量融合。这类是紧耦合的，算法中考虑了两者的互协方差。[11]而松耦合融合方式是以定常时间，将IMU得到的预测姿态和视觉传感器的已估计姿态在EKF框架中融合。[31]在基于优化的方法中，也可以将两部分紧密融合，即不需要来自相机的显式姿态估计。IMU驱动动力学方程（1）中的预测结果是高斯分布。预测结果和真实状态之间的误差被投射为由协方差加权的平方误差，然后作为正则化项加到损失函数中。[19]根据贝叶斯推理，正则化项被视为先验，而图像配准项被视为似然性。优化结果是后验分布，由纯图像配准的ML估计结果得到，然后IMU先验对这个估计结果进行平滑。

4.滤波方法与优化方法的联系

基于滤波和基于优化的方法都可以在贝叶斯推理下形成。经由近似线性化，两者的联系可以通过迭代EKF来明确表示。当近似线性化仅是单个步骤时，包含前通和后通的基于平滑的方法，等价于利用最小二乘问题中信息矩阵的Cholesky分解来求解的基于优化的方法。为了降低计算复杂度，通过保留仅有的关键帧或滑动窗口来减少要估计的状态变量。特别地，滑动窗的方案或滚动时域估计将损失函数划分为两部分，流行的方式是在一个部分中优化结果，在另一部分中用EKF来忽视过去的状态。

4.1 迭代EKF更新

基于滤波的方法核心是卡尔曼滤波器，而基于优化的方法核心是高斯-牛顿方法。它们之间可以通过迭代EKF（IEKF）[32]来联系。EKF有两个步骤：预测和更新。假设当前时刻k预测步骤的结果为

EKF和IEKF之间的区别在于，在IEKF的更新步骤中存在迭代循环，而在EKF的更新步骤中只执行单个循环。IEKF的迭代循环能使由模型线性化导致的误差尽可能接近基于优化的方法中的结果。接下来，我们将说明IEKF更新步骤中的迭代循环与基于最大化似然（ML）优化方法的高斯-牛顿方法两者是如何等价的。

4.2 基于平滑的方法

基于滤波的方法是序列迭代的过程，当前估计结果取决于当前测量和过去的估计结果（过去测量被精简在过去的估计结果中），即未来测量不对当前估计结果做任何贡献。反之，若未来的测量确实对当前估计结果做贡献，则属于应用了基于平滑的方法。这更像是一种批处理算法，所有测量都可用于估计。[33]

4.3 边缘化到关键帧

若如上述情形这样，保持操作过程中所有的状态变量和特征交会，那么随着探索轨迹的增加，计算复杂度会变得越来越大。然而，不是所有的传感器数据都是有用的。其中一些包含了关于跟踪和构图的关键信息，而另一些则是冗余的或不重要的信息可被忽略。有两种方法从过去的历史数据中选取有用的信息。一种是固定窗口方案，其仅保留最近的N个数据测量，而将N个测量之前的所有数据简单地边缘化。另一个是如图4所示，根据一些标准从整个数据集中选择N个数据测量。通过数据中的决定性信息来筛选关键帧是最普及的标准，在整个算法处理期间保留这些关键帧即可。[19,34]

4.3 滚动时域估计

滚动时域估计（MHE）方法将MAP损失函数分为两部分。如图4所示，在每个步骤中，只有从k步到步的最近N个项被优化，而最旧的项由EKF近似被概括为到达代价。

假设时间间隔为[0，k-N]和。 MAP问题是

前N项和是到达代价，MHE问题就是

其中是MHE窗口的开始时间，是在时刻的到达代价。到达代价由EKF近似为：

其中协方差P传播方式如下