VIO回顾：从滤波和优化的视角
http://www.5yedu.com/b64550.html
泡泡机器人翻译作品
作者：Javier Civera, DorianGálvez-López, L. Riazuelo, Juan D. Tardós and J.M.M.Montiel翻译：蔡纪源
审核：鲁涛
编辑：谢粤超 杨雨生
欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权
摘要
视觉惯性里程计(VIO, Visual inertial odometry)是一种使用移动平台自身传感器(相机及惯性测量单元)来估计其位置和旋转变化的技术。近年来，VIO吸引了大量的研究人员的关注。同时由于相机与IMU传感模块的尺寸小型化和成本降低，VIO也得以在各种潜在的应用中普及。然而，在技术开发和工程实现中往往要考虑算法的准确性，实时性，鲁棒性以及操作规模，因此具有很大的挑战性。本文从研究领域采用的两种主流方法，滤波和优化的角度报告最新的VIO技术的动态。为此，我们先举例展示了3D刚体运动体的各种表示方法，然后回顾基于滤波的方法，接着回顾基于优化的方法，并通过贝叶斯最大后验概率框架来阐明两种方法之间的联系。其他方面，我们也讨论了可观测性和自标定等问题。
1. 引言
定位与构图是机器人导航和控制研究领域的两个基本问题。多年的事实表明，同时定位与构图（SLAM）技术和运动恢复结构（SFM）技术[1]一直是机器人和计算机视觉社区的研究热门。近年来，人们越来越多地将这些技术应用到如小型无人机（UAV）或手持移动设备这样的小型平台上，相机和惯性测量单元（IMU）传感器渐渐成为定位和构图领域的研究焦点。得益于芯片和微机电系统（MEMS）器件工业的快速发展，这些传感器具有高精度，小型化和低成本等特点。而且它们彼此之间可以采用互相补充的方式，通过各个传感器提供的冗余信息来补偿它们自身的误差。此外，生物学的研究表明， 人类和一些动物的导航也会部分依赖运动感知形式和视觉的各种各样组合。[2-4]
在机器人社区中，定位与构图问题属于状态估计问题。主流使用的工具可以对给定噪声测量的动态系统进行随机估计，例如扩展卡尔曼滤波器（EKF）或粒子滤波器（PF）。 一些内部传感器可以测量姿势随时间的变化，这种变化关系可以用公式表示为移动平台的数据驱动动力学模型，包括地面移动机器人中的光学编码器和飞行机器人中的IMU，众所周知它们具有累积误差和测量偏差。 一些外部传感器，如相机或激光测距仪，能够提供角度或距离测量。基于三角测量或三边测量方法，这些传感器能够估计位置移动平台的位置和方向。 估计结果的可靠性在很大程度上取决于环境条件，而且结果对测量噪声敏感。 然而，在状态估计问题中，它们可以用测量模型来公式化表示。于是，EKF或PF能够通过融合动态模型中的分布和测量模型中的分布来推得从上一步到当前的估计分布。在大多数情况下，由于被估计状态数量过多，人们会用PF代替EKF，然后估计出高斯分布的均值和协方差。
在计算机视觉领域，由视觉进行运动估计是一个优化问题[5]。通过图像配准的迭代计算求得变化的相机姿态[6,7]。通常，从一幅图像中提取特征并重新投影到另一幅不同视角的图像中，这样，最小化特征与重投影特征的误差代价，就可以解算出相机相对结构化环境的姿态变化。基于优化的算法主要是基于梯度的，例如牛顿法或高斯-牛顿法。 多数情况下，我们可以利用矩阵的稀疏结构来提升计算效率。 优化的结果仅提供估计状态的值，而估计分布的信息是不可用的。
利用贝叶斯推理框架可以建立基于滤波和基于优化两种方法之间的联系。基于优化的方法，迭代地找到测量的总概率最高的状态，故可被看作为最大似然（ML）的思路。基于滤波的方法，其中平台姿态的先验分布由内部传感器的测量构建，并且似然分布由外部传感器的测量建立，故可以被视为最大后验（MAP）的思路。在非线性动态模型和/或非线性测量模型中，对EKF中的每个单步执行迭代更新，这样的迭代EKF等价于基于优化的方法。基于优化的方法，可以向内部传感器或其他源的测量中添加正则化项或先验项，使其从ML问题变为MAP问题。当前估计取决于当前和以前的测量，便是在线或“可溯”算法。当前估计取决于整个数据集，或者可以说它们不仅取决于当前和先前测量，而且还取决于之后的测量，这样的批处理过程，属于离线或“非可溯”算法。在基于优化的方法中，批处理过程被转为求解线性代数方程组。在基于滤波的方法中，卡尔曼平滑器能够经由前向通道和后向通道找到后高斯分布。
在VIO问题中，需要通过IMU和摄像机的给定测量来估计一长串的状态变量和参数。其中一些是不可观测或不可辨识的，会导致误差的增加。通过对状态和参数的可观测性和可辨识性的分析，我们可以进一步理解估计结果，并且潜在地提供了通过专用运动模式来充分激励平台的指南。
流行的SLAM技术能估计平台姿态和环境结构。需要通过闭环检测的过程来约束由估计算法引起的累积误差。然而，在VIO技术中没有明确地要求闭环检测[8-10]。 因此，VIO中的估计误差将被累积并且不能被约束。由于没有闭环检测，在小尺寸的装置内就可以进行长时间和大规模的定位。因此，研究重心也就转向了如何开发可以减慢误差累积的技术。
本文将介绍VIO技术最新的进步和发展。我们将从滤波和优化的角度组织全文，说明他们的基本模型，算法和最新结果。同时，为有助于深入了解VIO技术，我们突出讲解两种方法之间的联系。我们还将提供关于状态可观测性和参数可辨识性的讨论，并总结这一研究领域的愿景。
下面，第2节介绍基于滤波的方法，第3节介绍基于优化的方法。第4节总结它们之间的联系，可辨识性将在第5节中讨论。第6节将简要提供结论和未来的工作。
2.基于滤波的方法
EKF框架通常由预测步骤和更新步骤组成。对于基于滤波的VIO方法，惯性传感器测量得到的三轴加速度和转动速度，可用于计算3D刚体运动的数据驱动动力学模型或先验分布，并且可在预测步骤中用来进行运动预测。相机能够提供特征和移动平台之间的角度和距离测量，可用作测量模型或似然分布，并用于在更新步骤中更新预测。
我们假设这样一个移动平台，只配备一个相机和一个IMU，在未知的环境中移动。相机和IMU之间的空间关系是固定的，而且可以用已知的位姿表示。基于EKF的VIO算法的目的是使用惯性测量和对未知环境的视觉观测来提供平台的位置和方向信息。接下来，我们将基于[11]中的工作给出EKF框架的完整描述，包括状态描述，IMU数据驱动动力学和视觉观测。这将为后续章节的进一步分析和讨论作铺垫。
2.1.IMU数据驱动动力学模型
任意时刻的3D刚体的IMU状态向量可以由16×1向量定义：
其中 是单位四元数，用来描述从世界帧{W}到IMU帧{I}的旋转。 和 是关于{W}的位置和速度， 和 是3×1向量，分别用来描述影响陀螺仪和加速度计测量的偏差。帧间的空间关系如图1所示。
图1.坐标系，帧之间通过旋转 和平移p变换。
假设惯性测量中包含零均值高斯噪声（表示为 和 ），则实际角速度ω和实际加速度α与陀螺仪和加速度计测量值有以下关系：
数据驱动动力学模型是3D刚体动力学和上述IMU测量的组合，其可以通过以下形式表示：
其中 表示由 描述的旋转矩阵，g是世界坐标系{W}中的重力加速度，
是IMU帧{I}中的角速度。
是四元数运动学矩阵，其中 表示反对称矩阵。IMU偏差被建模为随机游走过程，由高斯噪声 和
表示。
将单位四元数表示为 ，其相应的旋转矩阵为 。两种旋转表示可以通过下式相联：
除了上述提到的当前IMU状态 ，基于滤波的方法广泛使用的状态向量还包括空间尺度λ（缓慢漂移）和当前相机姿态 。在这里，我们提出只包含一个相机姿态的系统状态，用一个24维的向量表示。然而，在不同方法中，整个系统状态通常包含多个过去的相机姿态，往往需要保持一个滑动窗口以在状态更新期间约束它们。
满足以下三个微分方程约束，
在上述方程中应用期望算子，使用IMU数据驱动动力学模型可以得到预测结果：
这可以被分离为带相机测量z和过程噪声项 〜N（0，Q）的非线性系统函数
2.2误差状态表示与更新
对于位置，速度和偏差状态变量，可以应用算术差（即，对量x，其估计量 的误差被定义为 ），但误差四元数应该在局部最小情况的假设下进行定义[12]。如果 是四元数 的估计值，则旋转误差由误差四元数 描述，其由
得到。 在该表达式中，符号⊗表示四元数乘法。直观地，四元数 描述了真实和估计的姿态之间的一个小的旋转。由于姿态对应的是三自由度（DoF），用δθ描述姿态误差是最小的表示。误差四元数 可写为
因此， 包含22个元素的误差状态向量是
连续时间误差状态的微分方程如下
其中
通过堆叠误差状态的微分方程，线性连续时间误差状态方程可以写成，
其中噪声向量 。并且n的协方差矩阵取决于IMU的噪声特性。
实际上，状态传播的惯性测量是以离散形式从IMU中获得的，因此我们假设来自陀螺仪和加速度计的信号是以时间间隔Δt采样的，并且使用数值积分方法（如Runge-Kutta方法）来传播状态估计。 此外，EKF滤波器的协方差矩阵被定义为
其中 是IMU状态的协方差矩阵， 是相机姿态估计的6N×6N协方差矩阵， 是IMU状态和相机姿态估计中的误差的相关性。有了这个表示，协方差矩阵就可以被传播，公式如下
其中状态转移矩阵可以通过假设 和 在两个连续步骤之间的积分时间间隔上为常量来计算，
离散时间协方差矩阵 也可以通过数值积分得到，
因此，基于EKF的VIO框架中的均值和协方差传播过程总结如下，
（a）当以固定采样频率采集IMU数据 和 用于滤波器时，可使用等式（1）上的数值积分来传播状态向量。
（b）分别根据等式（4）和（5）计算 和 。
（c）从等式（3）可计算传播状态协方差矩阵。
2.3 视觉测量模型与更新
由于IMU数据中存在偏差和噪声，预测步骤的预测结果会随着时间的增加而变得越来越差。视觉传感器的测量恰好能够提供关键信息以限制误差的增加。为了在EKF框架中用这方法，从图像中提取的关键信息要被转换成测量方程。有各种方法来构建测量模型。例如，松耦合方法中，通过图像配准直接获得位置和方向的变化，然后在EKF框架中融合两者估计结果。[13]而所谓的紧耦合方法则提倡使用从图像提取的关键信息。关键信息可以是通过特征检测器从图像中特征提取得到[14]，也可以由具有深度信息（点云）的直接光强度[15,16]与半直接光强度得到。[17]需要分析关键信息和状态变量之间的关系，使其建模为测量方程。通常，如下的非线性代数方程可以被视为测量方程：
其中 表示视觉测量中的高斯噪声，对应零均值和协方差R。
经过线性化之后，测量误差以线性形式表示：
其中H是是雅可比矩阵，噪声项是高斯分布的且与状态误差不相关。 和z分别表示预测和实际测量。
为了使用特征作为视觉信息，最直接的方式是利用针孔模型在相机帧{C}中建立测量方程。提取图像中的点 以表示关键信息。首先，相机帧中的点u和世界帧{W}中的点 之间的空间建立如下关系：
严格地说，视觉世界{V}不同于真实世界{W}，但它们通过如图1所示的预置的固定平移 和旋转 相连。这里，为了简单起见，我们只使用世界帧{W}。
图像帧中的位置通过等式（10）中描述的相机针孔模型链接到世界帧中的状态变量 和 。 因此，测量模型可以简单地表示为
在世界帧 中的该点的估计可以通过迭代最小化方法来计算得到。[18]
对于其他关键视觉信息，如像素强度或梯度，测量模型应先建立测量与状态变量之间的关系，然后再线性化非线性测量方程以获得雅可比矩阵。
此时，更新步骤已经准备好更新预测步骤中的预测结果。计算卡尔曼增益
最终修正 。在修正之后，我们可以获得更新的状态估计。 最后，误差状态协方差矩阵更新为
完整的更新过程总结如下：
（a）对于特定采样频率中的视觉数据，通常在原始图像上采用一些图像处理程序来提取关键信息。
（b）计算残差（7）。
（c）计算卡尔曼增益（8）。
（d）通过添加修正来更新状态。
（e）更新误差状态协方差矩阵（9）。
3. 基于优化的方法
基于优化的方法主要依赖于特征提取的图像处理技术和图像配准的最优化，而惯性测量被视为先验，可当作正则项或完全忽略。在大多数情况下，基于优化的方法有两个阶段：构图和跟踪。[15]在构图阶段，由各种特征检测器从图像中提取特征，如3D空间中的角点，边缘或其他地标。然后，对于检测到的所有特征，定义两帧图像之间的重投影误差。该误差用作要优化的损失函数，以便找到特征或地标的坐标。[7]在跟踪阶段中，地图中特征和地标的坐标被用于定义两帧图像之间的重投影误差，并且再次应用优化算法以计算出移动平台的位置和方向的变化。将估计问题分为两个阶段的想法可以获得快速的跟踪结果，然而构图处理还是耗时的。[14,15]对于上述分别由3D特征的坐标和移动平台的姿态改变定义的两个重投影误差损失函数，同时优化是可能的。[19]引入关键帧的概念能够边缘化旧的状态而保持一个有界优化窗口，保证实时性。
3.1特征配准
迭代非线性优化可以通过最小化图像观测区域的重投影误差来找到相机姿态改变和/或特征坐标。通常，地图由多个关键帧中标识的特征组成，即一系列的3D坐标向量表示，其中关键帧中包含着显著的特征。当获得新的图像时，应当判断其是否是关键帧。如果是，则将该新图像中通过图像配准算法计算找到的新特征坐标与当前相机姿态一起添加到地图中。否则，地图不更新。
如图2所示，3D刚体变换T∈SE(3)表示3D中的旋转和平移：
图像配准中优化的目的是在每个时间步长中找到变换T，即T被认为是相机姿态。用局部最小的形式表示相机姿态更适合用于优化。李代数se(3)，对应于SE(3)的切空间,作为局部最小表示。代数元素被称为扭曲坐标 。从李代数se(3)到李群SE(3)的映射是指数映射
其逆映射是对数映射
其中 是楔操作符，
相机帧中用齐次向量 表示的3D点通过针孔相机投影模型映射到图像坐标u有如下关系：
其中 , 和 , 分别是主点和焦距，表示可预先校准的相机固有参数。给定点u的深度信息 ，就可以从图像坐标恢复出相机帧中的3D点：
图2. 投影，3D点 被投影到由运动ξ链接的两个图像帧中。
从世界帧中的点 到相机帧中点的映射是 。对单目相机而言，视觉测量模型是 ，每个像素点的深度信息可以通过使用一组独立滤波器对两个连续关键帧进行三角测量获得。[14,20]
对于立体相机，
，也是ξ和 的函数。通过立体视觉技术获得深度信息。
重投影误差被定义为测量z与其估计
之间的差
损失函数 是带权重矩阵W的误差 的平方和
其中j从1到m是帧内的点的索引，i是从1到n的帧的数目。
最优化问题定义如下
用于解决上述问题的方法被称为光束平差法。[7]
在PTAM中，[14]优化过程被分为两个并行线程：构图和跟踪。给定跟踪结果 ，构图线程中的优化问题是：
给定构图结果 ，跟踪线程中的优化问题是：
在Levenberg-Marquardt（LM）算法中，关键是在每个迭代步骤中找到增量δ，然后更新优化的状态变量。 δ的解由下式得出：
其中H是z的雅可比，α是LM的阻尼参数。
在大多数情况下，计算中仅保留稀疏关键帧中的特征，以便将优化复杂度限制为与基于滤波的方法一样。[19]
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）优化方法的高斯-牛顿方法两者是如何等价的。
在k时刻，IEKF具有 和 ，分别对应当前状态、当前状态估计和测量。测量模型与等式（6）相同，且 。
定义二次损失函数中的误差向量如下，其带自由变量μ。
其中
那么最大似然优化问题就是
或者
给定初值 ，则用高斯-牛顿法迭代如下
其中 ，使用以上梯度的表达式，高斯-牛顿法迭代变为如下形式
其中增益为
协方差矩阵为
在i中的循环之后，可以看出来自更新步骤的结果是
总之，上述高斯 - 牛顿法的迭代循环被看作是IEKF的更新步骤，即
（a）初始化：i = 0， 和
（b）循环计算等式（11）到（13）;
（c）最终更新：
当只执行一次迭代循环时，上面便是EKF的更新步骤。它们的关系可以在图3中清楚地看到。
图3. 方法间的联系。 EKF，IEKF和平滑器三者的过程和关系。
4.2 基于平滑的方法
基于滤波的方法是序列迭代的过程，当前估计结果取决于当前测量和过去的估计结果（过去测量被精简在过去的估计结果中），即未来测量不对当前估计结果做任何贡献。反之，若未来的测量确实对当前估计结果做贡献，则属于应用了基于平滑的方法。这更像是一种批处理算法，所有测量都可用于估计。[33]
仍采用IMU数据驱动动力学方程（2）和相机测量方程（6），则全概率式子为
那么MAP问题就是
线性化 和 ，上述最小化问题被重写为线性方程组的形式：
这样问题就可以通过 的Cholesky分解来求解。[33]这是基于优化的批处理方法。另一方面，批处理可以从基于滤波的方法的角度来解释，即是基于随机处理的。
如图3所示，卡尔曼平滑器包含前向和反向通道。正向通道是使用迄今为止的数据来计算 ，而反向通道是使用所有的数据来计算 。
传统上，卡尔曼滤波器被表示为
经由线性化之后成为
其中， 仅考虑相对于xk的过去信息。如果我们要将它与未来的观测结合，那就可以计算出更精确的状态估计。这种结合过去和未来的估计器通常被称为平滑器，可被写为如下形式。
作为卡尔曼平滑器的扩展，可以使用EM算法来学习系统的参数。参数包括噪声协方差Q，R和F，H。
虽然上述算法包含两次通道传递，但未尝试针对非线性问题提出有效的线性近似。如果没有良好的线性化点，则会出现糟糕的结果。此外，随着轨迹越来越长，线性化方程的规模将难以管理。
4.3 边缘化到关键帧
若如上述情形这样，保持操作过程中所有的状态变量和特征交会，那么随着探索轨迹的增加，计算复杂度会变得越来越大。然而，不是所有的传感器数据都是有用的。其中一些包含了关于跟踪和构图的关键信息，而另一些则是冗余的或不重要的信息可被忽略。有两种方法从过去的历史数据中选取有用的信息。一种是固定窗口方案，其仅保留最近的N个数据测量，而将N个测量之前的所有数据简单地边缘化。另一个是如图4所示，根据一些标准从整个数据集中选择N个数据测量。通过数据中的决定性信息来筛选关键帧是最普及的标准，在整个算法处理期间保留这些关键帧即可。[19,34]
图4. 保留N个状态，边缘化和滚动时域估计。
边缘化状态等价于将Schur补应用于最小二乘问题。例如，
变为
经过这个正向替代步骤之后，较小的系统是
边缘化状态 将引起其他依赖于 的状态之间的依赖性问题，如 和 。并且从整个解决方案中边缘化最旧的姿态会需要在三个地方进行填充[35]：
1）在该姿态视野内的任意地标之间;
2）在下一个最旧姿态的状态之间;
3）在下一个最旧姿态和被删除姿态视野中的所有地标之间。
问题求解中的矩阵将变得密集并影响计算效率。
4.3 滚动时域估计
滚动时域估计（MHE）方法将MAP损失函数分为两部分。如图4所示，在每个步骤中，只有从k步到 步的最近N个项被优化，而最旧的项由EKF近似被概括为到达代价。
假设时间间隔为[0，k-N]和。 MAP问题是
前N项和是到达代价，MHE问题就是
其中 是MHE窗口的开始时间， 是在 时刻的到达代价。到达代价由EKF近似为：
其中协方差P传播方式如下
5.状态可观性和参数可辨识性
VIO要解决的问题是，给定惯性和视觉传感器的测量值，在全局帧中在线恢复出平台轨迹。然而，这个问题在实际实现方面不是直接解决的。除了基于滤波或基于优化的方法的复杂性之外，一些参数对表示平台轨迹的动态状态估计是否成功起到关键作用，这些参数包括：
•相机内在参数：焦距，主点和镜头畸变。
•IMU参数：加速度和陀螺仪偏差。
•空间参数：IMU和摄像机之间的变换关系。
•时间参数：IMU和摄像机测量之间的时间延迟
除了IMU偏差，它们都是不随时间变化的量，并称为系统参数。与之相反，平台轨迹由随时间变化的变量表示：状态。
分析可观测性和可辨识性，可以确定这些状态和参数是否可以通过仅有的IMU和相机测量数据来恢复出来。传统意义上，状态是动态变量，它们的估计问题使用可观测性来分析，而参数是静态变量，它们的标定则使用可辨识性来分析。对于IMU偏差而言，使用随机游走过程对参数建模能够使用可观测性来分析它们。
5.1 状态可观性
可观测性是反映输入/输出数据偏差的估计状态可能性的一种基本属性。令 表示从初始状态 ，初始时间 开始，连续时间系统的测量读数 对应的输出轨迹。对于所有的可采纳测量，当 ，如果 且 成立，则两个初始状态 和 是不可区分的。 如果状态是可区分的，则可以从输出和已知测量来估计出它们。如果状态是不可区分的，则被称为不可观测，他们对应的方差将会增长，且无界，变成无限大。
线性系统的可观测性是全局属性，既可以由可观测性矩阵的秩也可由Gramian矩阵的秩来确定。然而，非线性系统的可观测性是关于给定状态局部确定的。[36]局部可观测性强于可观测性，它能区分出状态和它们的邻近。而局部弱可观测性则能即时将状态与其邻近区分开，且没有大的偏移。[37]
局部弱可观测性的优点是可利用李导数代数检验。对于由等式（2）和（6）组成的非线性系统， 相对于 的Lie导数是
Lie导数还可以递归地定义
任何函数的零阶Lie导数是函数本身 。可观测性矩阵O由 的李导数行堆积而成。
如果可观测性矩阵O在 处列满秩，则系统在 处是局部弱可观测的。由于状态和测量方程是无限平滑的，因此可观测性矩阵可以具有无限行。如果可以找到足够多线性独立的行，则O是满秩的，且系统是局部弱可观测的。
许多论文根据能观性秩条件得出了结论，在全局帧中的平台姿态是不可观测的，且绕重力方向的旋转（偏航）也是不可观测的。其他一些状态是可观测的，包括在初始相机帧的平台姿态、重力向量，以及相机帧中的的特征。直观的解释是，视觉相机只是方位传感器，而IMU是姿态的双积分器，其不能在全局帧中提供姿态和偏航信息。
可观测性分析有助于我们找到充分激发系统的方法，例如使用具有世界坐标的三个非共线特征或者伪测量方程来使一些原本不可观测的变得可观测。[1]此外，它有可能为如何使不可观测的无穷大尽可能变慢问题提供线索。
对于基于滤波的方法，在EKF中采用线性化系统。然而，线性化系统的可观测性不同于相应的非线性系统。更具体地说，在线性化系统中偏航是可观测的，这个超出预期的可观测自由度会导致不一致的结果：估计的协方差优于实际测量结果。改进一致性的一个办法是在模型线性化导致的错误可观测方向上施加约束。[38]
可观测性还可以用连续对称性的概念来分析，其能够找到可观测状态和可辨识参数的分析推导。[39]
5.2 参数可辨识性
在大多数视觉相关的定位技术中，相机内参是预先已知的。它们也可以如[40]中介绍的方法进行在线校准。[40]
IMU偏差会随时间变化，在大多数情况下被建模为时变状态。能观性秩条件表明它们是可观测的。
IMU和相机之间的空间参数是六自由度的变换，必须准确地知道才能稳定估计结果。它们被建模为随机游走过程，并被视为时变状态。能观性秩条件的分析显示，六自由度相机IMU的变换，以及IMU偏差，重力向量和度量场景结构全都是可观察的。完全可观察性需要充分的激励，即平台至少沿着两个IMU轴线进行旋转和加速。[41-43]
惯性和视觉传感器两者的测量是具有时延。时间延迟的局部可辨识性通过构造包含时间延迟和其他量的约束方程来确定。通过检查约束条件的雅可比矩阵秩来得到局部可辨识性。结果表明，时间延迟在一般轨迹中是局部可辨识的，但一些关键的轨迹具有使可辨识性丢失的特点。[44]惯性和视觉传感器之间的测量时间延迟也可由变种ICP算法估计得到。[45]
6. 结论
本文提出了最先进的视觉惯性里程计方法的概述。从基于滤波和基于优化两个角度的方法讲述，并描述了处理视觉图像的两种方式：基于特征的和基于稠密的。将EKF和图像配准的广泛知识统一到同一个框架中，两种方法之间的联系用迭代EKF、平滑器和边缘化来表征，并揭示了对两种方法的深入了解。 还分析了状态的可观测性和参数的可辨识性。本文还总结了本研究领域中的一系列技术，包括EKF，MAP，IEKF，BA，ICP，MHE等。在将来，探索这些方法的变种的计算复杂度，将为该领域的从业者提供更直观的指导以及更多的实施细节。
参考文献
(蓝色区域滑动浏览全部文献)
[1] Chiuso A, Favaro P, Jin H, et al. Structure from motion causally integrated over time. IEEE Trans. Pattern Anal. Mach. Intell. 2002;24:523–535.
[2] Wolf H. Odometry and insect navigation. J. Exp. Biol.
2011;214:1629–1641.
[3] Corke P, Lobo J, Dias J.An introduction to inertial and visual sensing. Int. J. Rob. Res. 2007;26:519–535.
[4] Franceschini N, Pichon J-M, Blanes C, et al. From insect vision to robot vision [and discussion]. Philos. Trans. R. Soc. B: Biol. Sci. 1992;337:283–294.
[5] Scaramuzza D, Fraundorfer F. Visual odometry [tutorial].
IEEE Rob. Autom. Mag. 2011;18:80–92.
[6] Baker S, Matthews I. Lucas-Kanade 20 years on: a unifying framework. Int. J. Comput. Vision. 2004;56:221–255.
[7] Triggs B, McLauchlan PF, Hartley RI, et al. Bundle adjustment – a modern synthesis. In: Vision algorithms: theory and practice. Berlin:Springer; 2000. p. 298–372.
[8] Durrant-Whyte H, Bailey T. Simultaneous localization and mapping: part I. IEEE Rob. Autom. Mag. 2006;13:99–110.
[9] Bailey T, Durrant-Whyte H. Simultaneous localization and mapping (SLAM): part II. IEEE Rob. Autom. Mag. 2006;13:108–117. Advanced Robotics1301
[10] Dissanayake MG, Newman P, Clark S, et al. A solution to the simultaneous localization and map building (SLAM) problem. IEEE Trans. Rob. Autom. 2001;17:229–241.
[11] Li M, Mourikis AI. High-precision, consistent EKF-based visual-inertial odometry. Int. J. Rob. Res. 2013;32:690–711.
[12] Li M, Mourikis AI. Improving the accuracy of EKF-based visual-inertial odometry. In: IEEE International Conference on Robotics and Automation (ICRA) Saint Paul, MN, USA; 2012. p. 828–835.
[13] Weiss S, Achtelik MW, Lynen S, et al. Monocular vision for long-term micro aerial vehicle state estimation: a compendium. J. Field Rob. 2013;30:803–831.
[14] Klein G, Murray D. Parallel tracking and mapping for small AR workspaces. In: 6th IEEE and ACM International Symposium on Mixed and Augmented Reality (ISMAR); Nara, Japan 2007. p. 225–234.
[15] Newcombe RA, Lovegrove SJ, Davison AJ. DTAM: dense tracking and mapping in real-time. In: IEEE International Conference on Computer Vision (ICCV) Barcelona, Spain; 2011. p. 2320–2327.
[16] Henry P, Krainin M, Herbst E, et al. RGB-D mapping: using Kinect-style depth cameras for dense 3D modeling of indoor environments. Int. J. Rob. Res. 2012;31:647–663.
[17] Engel J, Schöps T, Cremers D. LSD-SLAM: large-scale direct monocular SLAM. In: European Conference on Computer Vision (ECCV) Zurich, Switzerland; 2014.
[28] Pizzoli M, Forster C, Scaramuzza D. REMODE: proba-bilistic, monocular dense reconstruction in real time. In: IEEE International Conference on Robotics andAutomation (ICRA) Hong Kong, China; 2014. p. 2609–2616.
[29] EngelJ,SturmJ,CremersD.Semi-densevisualodometryfor a monocular camera. In: IEEE International Conference on Computer Vision (ICCV) Sydney, Australia; 2013. p. 1449–1456.
[30] Forster C, Pizzoli M, Scaramuzza D. SVO: fast semi-direct monocular visual odometry. In: IEEE International Conference on Robotics and Automation (ICRA). Hong Kong, China; 2014. p. 15–22.
[31] Weiss S, Scaramuzza D, Siegwart R. Monocular-SLAM –based navigation for autonomous micro helicopters in GPS-denied environments. J. Field Rob. 2011;28:854–874.
[32] Bell BM, Cathey FW. The iterated Kalman filter update as a Gauss–Newton method. IEEE Trans. Autom. Control. 1993;38:294–297.
[33] Kaess M, Ranganathan A, Dellaert F. ISAM: incremental smoothing and mapping. IEEE Trans. Rob. 2008;24:1365–1378.
[34] Strasdat H, Montiel JM, Davison AJ. Visual SLAM: why filter? Image Vision Comput. 2012;30:65–77.
[35] Sibley G, Matthies L, Sukhatme G. A sliding window filter for incremental SLAM. In: Unifying perspectives in computational and robot vision; Berlin: Springer, 2008.
p. 834–849.
p. 103–112.
[18] Mourikis AI, Roumeliotis SI. A multi-state constraint Kalman filter for vision-aided inertial navigation. In: IEEE International Conference on Robotics and Automation (ICRA) Rome, Italy; 2007. p. 3565–3572.
[19] Leutenegger S, Furgale PT, Rabaud V, et al. Keyframe-based visual-inertial SLAM using nonlinear optimization [Internet]. In: Robotics: science and systems VI; 2013. Available from http://www.roboticsproceedings.org/rss06/ index.html
[20] Strasdat H, Montiel J, Davison AJ. Scale drift-aware large scale monocular SLAM. Rob.: Sci. Syst. 2010;2:5.
[21] ComportAI, Malis E, Rives P.Accurate quadrifocal tracking for robust 3D visual odometry. In: IEEE International Conference on Robotics and Automation (ICRA) Rome, Italy; 2007. p. 40–45.
[22] Steinbrucker F, Sturm J, Cremers D. Real-time visual odometry from dense RGB-D images. In: IEEE International Conference on Computer Vision Workshops (ICCV Workshops) Barcelona, Spain; 2011. p. 719–722.
[23] Audras C, Comport A, Meilland M, Rives P. Real-time dense appearance-based SLAM for RGB-D sensors. In: Australasian Conference on Robotics and Automation Melbourne, Australia; 2011.
[24] Tykkala T, Audras C, Comport AI. Direct iterative closest point for real-time visual odometry. In: IEEE International Conference on Computer Vision Workshops (ICCV Workshops) Barcelona, Spain; 2011. p. 2050–2056.
[25] Besl PJ, McKay ND. A method for registration of 3-D shapes. IEEE Transactions on PatternAnalysis and Machine Intelligence. 1992;14:239–256.
[26] Dryanovski I, Valenti RG, Xiao J. Fast visual odometry and mapping from RGB-D data. In: IEEE International Conference on Robotics andAutomation (ICRA) Karlsruhe, Germany; 2013. p. 2305–2310.
[27] Newcombe RA, Izadi S, Hilliges O, et al. KinectFusion: real-time dense surface mapping and tracking. In: 10th IEEE International symposium on Mixed and augmented reality (ISMAR) Basel, Switzerland; 2011. p. 127–136.
[36] Hermann R, Krener AJ. Nonlinear controllability and observability. IEEE Trans. Autom. Control. 1977;22:728–
740.
[37] Sontag ED. A concept of local observability. Syst. Control Lett. 1984;5:41–47.
[38] Guo CX, Roumeliotis SI. IMU-RGBD camera 3D pose estimation and extrinsic calibration: Observability analysis and consistency improvement. In: IEEE International Conference on Robotics andAutomation (ICRA) Karlsruhe, Germany; 2013. p. 2935–2942.
[39] Martinelli A. Visual-inertial structure from motion: observability and resolvability. In: IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Tokyo, Japan; 2013. p. 4235–4242.
[40] Civera J, Bueno DR, Davison AJ, et al. Camera self-calibration for sequential Bayesian structure from motion. In: IEEE International Conference on Robotics and Automation (ICRA). Kobe, Japan; 2009. p. 403–408.
[41] Kelly J, Sukhatme GS. Visual-inertial sensor fusion:
localization, mapping and sensor-to-sensor self-calibration. Int. J. Rob. Res. 2011;30:56–79.
[42] Mirzaei FM, Roumeliotis SI. A Kalman filter-based algorithm for IMU-camera calibration: observability analysis and performance evaluation. IEEE Trans. Rob. 2008;24:1143–1156.
[43] Jones ES, Soatto S. Visual-inertial navigation, mapping and localization:ascalablereal-timecausalapproach.Int.J.Rob. Res. 2011;30:407–430.
[44] Li M, Mourikis AI. Online temporal calibration for camera-IMU systems: theory and algorithms. Int. J. Rob. Res. 2014;33:947–964.
[45] Kelly J, Sukhatme GS. A general framework for temporal calibration of multiple proprioceptive and exteroceptive sensors. In: Experimental robotics. Berlin:Springer; 2014.
p. 195–209.