论文翻译：Keyframe-Based Visual-Inertial Odometry Using Nonlinear Optimization

摘要：

        结合视觉和惯性测量已经在移动机器人中变得流行，因为这两种传感模式提供了互补的特性，使其成为精确的视觉惯性里程计或同时定位和建图（SLAM）的理想选择。虽然历史上问题已通滤波器得到解决，但视觉估计的进步表明非线性优化提供了更高的准确性，同时由于潜在问题的稀疏性，问题的复杂性仍然易于处理。从这些发现中获得灵感，我们制定了严格的结合路标和惯性项的重投影误差的概率成本函数。该问题易于处理，从而通过边缘化将优化限制在关键帧的有界窗口，从而确保实时操作。关键帧可以按时间间隔任意间隔，同时仍然与线性化惯性项相关。我们使用我们定制的立体视觉惯性硬件记录互补数据集的评估结果，该硬件可以将加速度计和陀螺仪测量与图像精确同步。关于地面实况，显示了在有和没有在线外部估计的情况下我们的算法的立体和单眼版本的比较。此外，我们将性能与最先进的stochasic克隆滑动窗口滤波器的实现进行比较。该竞争参考实现执行基于紧耦合滤波的视觉惯性测距。虽然我们的方法声称需要更多的计算，但我们在准确性方面表现出了卓越的性能。

简介

        视觉和惯性测量提供互补特性，使其特别适合融合，以解决稳健和精确的定位和映射，这是任何移动机器人系统的主要需求。 投影到图像中的结构的丰富表示，以及IMU中包含的陀螺仪和加速度计的精确短期估计已经被认为是相互补充的，具有良好的空中（i and Mourikis, 2012a）和汽车用例（Li和Mourikis，2012a） 此外，随着大多数智能手机中这些传感器的出现，人们对视觉惯性SLAM的有效解决方案产生了极大的兴趣和研究活动（Li et al。，2013）

        从历史上看，在处理视觉惯性估计问题方面存在两个主要概念：批量非线性优化方法和递归滤波方法。 虽然前者联合最小化了来自集成IMU测量的误差和来自视觉术语的（重投影）误差（Jung和Taylor，2001），但递归算法通常使用IMU测量来进行状态传播，而更新源于视觉观察（Chai等人）。 。，2002; Roumeliotis等，2002）。

        批处理方法提供了对视觉 - 惯性状态估计问题中涉及的固有非线性成本项的重复线性化的优点，因此它们限制了线性化误差。 然而，长期以来，缺乏计算资源使得递归算法成为在线估计的有利选择。 然而，这两种范式最近都显示出对另一种范式的改进和妥协，因此最近的工作（Leutenegger等，2013; Nerurkar等，2013; Indelman等，2012）显示基于批处理的算法达到实时操作和基于滤波器的方法，提供几乎相同质量的结果（Mourikis和Roumeliotis，2007; Li等，2013）。 撇开计算需求，基于批处理的方法可以提供与滤波器方法相比更高精度的结果，因为本文后面将详细讨论固有的算法差异。

        除了分批和滤波器之外，文献中的视觉 - 惯性融合方法可以分为两类：松耦合系统通过仅视觉算法独立估计姿态，仅在单独的估算步骤中融合IMU测量，限制计算复杂性。相反，紧耦合方法包括来自IMU和摄像机的测量结果，这是共同估计所有状态的共同问题，因此考虑它们之间的所有相关性。然而，两种方法的比较表明（Leuteneggeret al。，2013）这些相关性是任何高精度视觉惯性导航系统（VINS）的关键，这也是为什么最近提出的所有高精度视觉惯性估计都实现了紧耦合VINS的原因。例如Mourikis和Roumeliotis（2007）提出了一种基于扩展卡尔曼滤波器（EKF）的单目视觉实时融合，称为多状态约束卡尔曼滤波器（MSCKF）。这项工作令人印象深刻，开环误差低于行进距离的0.5％。因此，我们将我们的结果与Mourikis等人发表的具有动态特征边缘化的滑动窗口滤波器的竞争实现进行比较。 （2009年）。为了更简单的参考我们在本文的其余部分用“MSCKF”表示这个算法，记住可用的参考实现不包括所有可能的修改（Li和Mourikis，2012a，b; Li等人，2013; Hesch等人。，2013）。

        在本文扩展了我们之前的工作（Leutenegger等，2013），我们提出了一种尊重上述发现的方法：我们提倡紧耦合融合，以便尽可能地利用所有测量和非线性优化而不是滤波器， 为了减少线性化导致的次优性。 此外，优化方法允许采用稳健的成本函数，即使在应用复杂的拒绝方案之后，也可能在存在可能偶尔出现在视觉部分中的异常值的情况下显着地提高准确度。

        我们设计了一种cost function，它以完全概率的方式结合了视觉和惯性术语。我们采用关键帧的概念，因为它成功应用于经典的仅视觉方法：它是使用部分线性化和边缘化实现的，即可变消除 - 对实时合规性和易处理性进行滤波的折衷。关键帧范例也考虑了无抖动估计，当存在慢速或无运动时：不是使用时间连续位姿的优化窗口，我们保持的关键帧可以在时间上任意间隔，保持视觉约束 - 同时仍然包含IMU术语。尽管我们的参数化使用了全局坐标，但我们在关键帧之间的相对不确定性的表达从RSLAM（Mei et al。，2011）中获得灵感。我们提供IMU误差项和相应信息矩阵的严格概率推导，将连续图像帧相关联，而无需以IMU速率明确地引入状态。在系统级，我们开发了用于精确实时SLAM的硬件和算法，包括使用惯性线索的鲁棒关键点匹配，自举和异常值拒绝。

图1显示了在室内数据集上运行的立体视觉惯性测距算法的输出：立体视觉和IMU传感器在ETH主楼的几个楼层和楼梯中行走了470米。 除了由位姿，速度和IMU偏差组成的状态外，我们还获得了环境的印象，表示为3D地标的稀疏地图。 请注意，由于紧密耦合的IMU融合，地图和路径会自动与重力对齐。

关于会议文件（Leutenegger等，2013），我们做出以下主要贡献：

• 在展示了与松散耦合方法相比所建议方法的优越性能之后，我们提出了关于随机克隆滑动窗口滤波器的广泛评估结果（遵循Mourikis等人（2009）的MSCKF实施，其中包括第一次 - 估计Jacobians）在不同运动轮廓的准确性方面。 我们的算法始终优于基于滤波器的方法，但它确实会带来更高的计算成本。 据我们所知，不同研究组提出的视觉 - 惯性状态估计算法的这种直接比较对于该领域是新颖的。
• 我们的框架已经扩展到与单目摄像机设置一起使用。 我们提出了有关估计和bootstrapping部分的必要调整。 需要单眼版本才能与MSCKF算法的参考实现进行公平比较，MSCKF算法目前仅以单眼形式发布。 结果是通用的N相机（N≥1）视觉惯性测距框架。 在立体声版本中，当摄像机基线与距离结构之间的比例时，性能将逐渐转变为单目情况变小。
  
• 我们提出了在线相机外参估计的公式，可以在标准内在校准后应用。 评估结果证明了当使用相对于IMU的不准确的相机姿态估计进行初始化时该方法的适用性。
  
• 我们诚实地尝试将我们的工作呈现出一定程度的细节，使读者能够重新实现我们的框架。
  
• 我们的新硬件迭代（从手持室内运动到骑自行车）记录了各种具有运动，外观和场景深度各个特征的新数据集。 由于更好的校准和硬件同步，以及算法和软件级调整，综合评估与我们之前发布的结果相比表现出优越的性能。

本工作的其余部分结构如下：在第二部分中，我们提供了一个更详细的概述，说明我们的工作如何与现有文献相关并区分自己。 第III节介绍了本文中使用的符号和定义。 深入描述了来自摄像机和IMU测量的非线性误差项。 在第IV节中，接着是第V节中前端处理和初始化的概述。作为该方法的最后一个关键元素，第VI节介绍了如何通过边缘化应用关键帧概念。 第七节描述了实验装置，评估方案，并在不同的数据集上提供了广泛的结果。

Relaeted Work

        构成今天VINS基础的仅视觉算法可以分为批SfM和基于滤波器的方法。 由于计算限制，长期以来，基于视觉的实时测距法或SLAM算法（例如Davison（2003）中提出的算法）只能使用滤波器方法。 然而，随后的研究（Strasdat et al。，2010）表明，与滤波器方法相比，基于非线性优化的方法，如通常用于离线SfM，可以为类似的计算工作提供更好的准确性，因为结构稀疏性是 问题得以保留。 从此以后，维持一个相对稀疏的关键帧图及其相关的地标受到非线性优化一直很受欢迎（Klein和Murray，2007）。

        VINS的最早结果源于Jung和Taylor（2001）对（基于样条）批次和Chai等人的研究。（2002年）; Roumeliotis等。 （2002）基于滤波器的方法。 随后，基于EKF发布了各种基于滤波器的方法（Kim和Sukkarieh，2007; Mourikis和Roumeliotis，2007; Li和Mourikis，2012a; Weiss等，2012; Lynen等，2013），Iterated EKFs （IEKFs）（Strelow和Singh，2004年，2003年）和Unscented卡尔曼滤波器（UKFs）（Shin和El-Sheimy，2004; Ebcin和Veth，2007; Kelly和Sukhatme，2011）仅举几例，这些年来表现出来 精度的显着提高和计算复杂性的降低。 今天，这样的6个DoF视觉惯性估计系统可以在消费者移动设备上在线运行（Li和Mourikis，2012c; Li等，2013）。

        为了限制计算复杂性，许多工作遵循松散耦合方法。 Konolige等。 （2011）使用立体视觉测量将IMU测量作为独立测斜仪和相对偏航测量集成到优化问题中。 相比之下，Weiss等人。 （2012）使用仅视觉位姿估计作为具有间接IMU传播的EKF的更新。 对于松散耦合的基于批处理的算法，可以遵循类似的方法，例如在Ranganathan等人的文献中。 （2007年）和Indelman等人。 （2012），其中相对立体姿态估计被集成到具有非线性优化的因子图中，包括惯性项和绝对GPS测量。 众所周知，松散耦合方法本质上是次优的，因为它们忽略了不同传感器的内部状态之间的相关性。

        基于过滤的VINS领域的一个显着贡献是Mourikis和Roumeliotis（2007）的工作，他们提出了一种基于EKF的单目视觉实时融合，称为多状态约束卡尔曼滤波器（MSCKF），它执行非线性三角测量。在EKF更新中使用它们之前，一组相机中的地标构成了一段时间。这与仅使用成对相机位姿之间的视觉约束的其他作品形成对比（Bayard和Brugarolas，2005）。 Mourikis和Roumeliotis（2007）还展示了如何消除地标误差与摄像机位置之间的相关性 - 这是通过使用估计的摄像机构成三角剖分而引入的 - 因此可以得到一个估计器，该估计器的线性误差是一致的和最优的。 Jones和Soatto（2011）提出了另一种单眼视觉惯性滤波器，它在长的室外轨迹上展示了结果，包括IMU到摄像机校准和环路闭合。 Li和Mourikis（2013）表明，通过在MSCKF中使用的地标处理模型与EKF-SLAM所采用的地标的完全估计之间切换，可以实现MSCKF性能的进一步提高。

        基于松散和紧耦合滤波的方法的进一步改进和扩展包括替代旋转参数化（Li和Mourikis，2012b），包括滚动快门相机（Jia和Evans，2012; Li等，2013），离线（Lobo） 和Dias，2007; Mirzaei和Roumeliotis，2007年，2008年）和在线（Weiss等，2012; Kelly和Sukhatme，2011; Jones和Soatto，2011; Dong-Si和Mourikis，2012）校准相对位置和方向 相机和IMU。

        为了从批量优化中的重新线性化提供的更高精度中受益，最近的工作集中于近似批量问题以允许实时操作。保持问题易于在线评估的方法可分为三组（Nerurkar等，2013）：首先，增量方法，如Kaess等人的基于因子图的算法。 （2012）;布赖森等人。 （2009），对问题进行增量更新，同时将优化问题或测量雅可比矩阵的相关信息矩阵分解为平方根形式（Bryson等，2009; Indelman等，2012）。其次，固定滞后平滑器或滑动窗口滤波器方法（Dong-Si和Mourikis，2011; Sibley等人，2010; Huang等人，2011）仅考虑优化中固定时间间隔的位姿。落在窗外的位姿和地标被边缘化，其相应的测量结果被丢弃。然而，在边缘化步骤中形成不同优化参数之间的非线性约束会破坏问题的稀疏性，使得窗口大小必须保持相当小以实现实时性能。然而，窗口越小，重复再线性化的益处越小。第三，基于关键帧的方法通过仅保留相机位姿和地标的子集并且丢弃（而不是边缘化）中间量来保持稀疏性.Nerurkar等人。 （2013）提出了一种有效的离线MAP算法，该算法使用来自非关键帧和地标的所有信息，通过边缘化一组帧和地标来形成关键帧之间的约束，而不会影响问题的稀疏性。虽然与完整批量MAP估算器相比，这种边缘化形式显示出较小的错误，但我们的目标是具有适合在线和实时操作的固定窗口大小的版本。在本文和我们之前的工作（Leutenegger等，2013）中，我们因此从非关键帧中删除测量结果并将各自的状态边缘化。当关键帧随时间退出窗口时，我们边缘化各个状态和通常观察到的一些界标，以形成优化问题的剩余子部分的（线性）先验。我们的近似方案严格保持原始问题的稀疏性。这与例如Sibley等人。 （2010年），由于边缘化而接受一些稀疏性损失。后一种滑动窗口过滤器，在视觉惯性变量中，用于Li和Mourikis（2012a）的比较：它证明比原始MSCKF表现更好，但有趣的是，使用初步估计雅可比人的改进MSCKF变体产生更好结果。我们的目标是在MSCKF实现（包括使用首次估计雅可比行列式）和我们的关键帧以及基于优化的算法之间进行类似的比较。

        除了批处理和过滤方法之间的区别之外，通过研究VINS的可观察性来提高估计精度是一个主要的兴趣。鉴于传感器或测量的特定组合（Martinelli，2011; Weiss，2012）或仅使用来自减少的IMU轴组的数据（Martinelli，2014），可观察性属性有大量工作。偏航和位置的全局不可观测性以及关于初始参考位姿的不确定性增加是视觉 - 惯性估计问题所固有的（Hesch等，2012b; Huang等，2013; Hesch等，2013） ）。因此，在将过滤方法与批量算法进行比较时，此属性特别重要：位姿的表示及其在全局参照系中的不确定性通常在数值上存在问题，因为部分状态的不确定性经历无限增长，而对于可观察到的国家的子部分。因此，我们的批处理方法使用关键帧的相对不确定性的公式来避免表达全局不确定性。

        VINS问题的不可观察性对过滤方法提出了特别的挑战，其中通常不可能重复线性化：Huang等。 （2009）已经表明，这些线性化误差可能错误地使估计状态的部分在数值上可观察到。 Hesch等。 （2012a）和其他人（Huang等人，2011; Kottas等人，2012; Hesch等人，2012b，2013; Huang等人，2013）衍生的配方允许选择VINS系统的线性化点。 方式使得线性化和非线性系统的可观察性属性相等。 在我们提出的算法中，我们使用第一估计雅可比行列式，即每当采用变量的线性化时，我们为涉及该特定变量的任何后续线性化固定线性化点。

３．NOTATION AND DEFINITIONS

A声明

我们在整个工作中使用以下符号：FA表示参考帧A；参考帧A中一点P在齐次坐标系表示为，FA到FB的变换表示为TAB, 旋转矩阵写作CAB ,相应的四元数qAB ,定义两种符号：

B关键帧

使用图2中示意性描绘的IMU和相机设置来评估所提出的方法的性能。它用于单目和立体模式，其中我们想要强调我们的方法足够通用以处理N相机设置。在相对于惯性框架FW表示的跟踪体内，我们区分相机帧FCi（下标为i = 1,...N），以及IMU传感器帧FS.

C状态

要估计的变量包括图像时的机器人状态（index k）xkR和地标xL。 xR将机器人位置保持在惯性框架WrS，车身方向四元数qWS，在传感器框架SvWS中表示的速度（简写为Sv），以及陀螺仪的偏差bg和加速度计偏差ba 因此，xR写成：

　　　　　　

第j个路标的其次坐标位：在该点将第四个组件设置为１

可选地，我们可以包括相机外部学估计作为在线校准过程的一部分。 相机外外参可以被视为要校准的恒定实体，或者经受一阶高斯过程的时变状态，允许跟踪可能发生的变化，例如， 由于温度引起的机械变形。

        一般来说，各状态存在于在一个流形中，因此我们在切线空间g中使用扰动并使用群运算符。这通常不是可交换的，指数exp和对数log。现在，我们可以在估计x附近定义扰动，我们使用最小坐标表示,双射映射从最小坐标变换到切线空间。因此，我们从最小坐标获得转换：

具体地说，我们使用方向的最小（3D）轴角度扰动，可以通过指数映射将其转换为四元数等价δq

因此，使用组操作符⊗，我们记记δα= 0附近的指数映射的线性化化：

其中t表示身份四元数。 我们获得最小的机器人错误状态向量

类似于机器人状态分解xT和xsb，我们使用位姿误差状态，和速度/偏差错误状态作为具有里程碑意义的扰动，我们使用简单的欧氏版本，用作

IV. 具有惯性的批量视觉SLAM

在本节中，我们介绍了将惯性测量结合到批量可视SLAM中的方法。 在视觉测距和SLAM中，通过最小化在相机帧中观察到的地标的重投影误差，制定非线性优化以找到相机位姿和界标位置。 图3显示了受（Thrun和Montemerlo，2006）启发的相应图形表示：它将测量值显示为具有方框的边缘，并将估计量显示为圆形节点。 一旦引入惯性测量，它们不仅会在连续位姿之间产生时间约束，

而且，在加速度计和陀螺仪的连续速度和IMU偏差估计之间，还增加了机器人状态向量。

        我们试图将视觉惯性定位和映射问题表达为包含来自IMU的（加权）重投影误差er和（加权）时间误差项的成本函数J(x)的一个联合优化：

其中i是组件的摄像机索引，k表示摄像机帧索引，j表示地标索引。 在第k帧和第i个相机中可见的界标的索引被写为集合J（i; k）。此外，W(i, j,k r)表示相应地标测量的信息矩阵，并且Wks是第k个IMU误差的信息。

在我们的工作中，我们使用与我们的实时C ++软件基础架构集成的Google Ceres优化器（Agarwal等，n.d。）。
在下文中，我们将介绍重投影误差公式。 然后，给出了IMU运动学与偏差项建模相结合的概述，在此基础上我们建立了IMU误差项。

A.重投影误差表示

我们使用相当标准的重投影误差公式，并修改了Furgale（2011）的微小修改：

因此，hi(.)表示相机投影模型（可包括失真）和zi, j, k表示测量图像坐标。 我们也在这里提供雅克比矩阵，因为它们不仅需要有效解决，而且在第六节中解释的边缘化步骤中发挥核心作用：

其中Jr,i表示相对于均匀地标中的地标的投影hi(.)进入第i个相机（包括失真）的雅可比矩阵，带有上划线的变量代表我们当前的猜测。 我们的框架目前支持径向切向和等距离失真模型。

B. IMU运动学和偏差模型

        在能够制定非线性IMU项之前，我们概述了描述IMU运动学和偏差演化的微分方程。 该模型通常用于估算IMU（Savage，1998），使用类似于MEMS-IMU的简化（Shin和El-Sheimy，2004）。
1）非线性模型：假设地球自转的测量效果与陀螺仪精度相比较小，我们可以将IMU运动学与简单的动态偏差模型结合起来，如下：

其中的元素是不相关的零均值高斯白噪声过程。 Sa是加速度计测量值，Wg表示地球的重力加速度矢量。 将陀螺仪偏差建模为随机游走，相反，将加速度计偏差建模为时间常数τ> 0的有界随机游动。矩阵Ω由估计的角速率形成，陀螺仪测量值：

2）误差状态的线性化模型：围绕xR的上述等式的线性化版本将在边缘化步骤中起主要作用。 因此，我们在此简要回顾一下：错误动态采取的形式

其中G是直接派生的，并且：

其中[.]×表示与向量关联的斜对称叉积矩阵。 Overbars通常用于评估具有当前估计的相应符号。

C. IMU测量误差项的制定

图4示出了在时间步长k和k + 1下进行的相机测量的测量速率的差异，以及不一定与相机测量同步的更快的IMU测量。 注意引入本地时间索引r = 0;...; 相机测量之间的R，以及相应的时间增量Δtr。

我们需要IMU误差项 是步骤k和k + 1处的机器人状态以及这些时间实例之间的所有IMU测量值的函数（包括加速度计） 和陀螺仪读数）总结为zk s。 因此，我们必须假设在相机测量k和k + 1的给定机器人状态下的近似正常条件概率密度f：

我们采用上面的传播方程来制定预测 具有相关的条件协方差我们应用经典的Runge-Kutta方法，以获得离散时间非线性状态转移方程fd(xkR)和误差状态转移矩阵Fd(xkR)。后者是通过将δχ_R= Fc(xR)δχR与Δtr保持一致而得到的，保持δχR符号。

使用预测，我们现在可以将IMU错误术语表示为：

这只是基于先前状态的预测与实际状态之间的差异 - 除了方向，我们使用简单的乘法最小误差。

接下来，在应用误差传播法则时，相关信息矩阵Wk s为：

雅可比很容易获得但非平凡，因为定向误差通常是非零的：

最后，对于优化问题的有效求解，将需要关于δχkR和δχkR+ 1的雅可比行列式。 虽然相对于δχkR+ 1的区分是直截了当的（但是非平凡的），但是对其他雅可比行列式给予了一些关注。 回想一下，通过迭代地应用预测来计算IMU误差项（17）。 因此，关于状态δχkR的微分导致链规则的应用，服从：

V. 前面的概述

本节概述了图像处理步骤和数据关联以及地标和状态的异常值检测和初始化。

A.关键点检测，匹配和变量初始化
        我们的加工管道采用定制的多尺度SSE优化Harris角点检测器（Harris和Stephens，
1988）随后是BRISK描述符提取（Leutenegger等，2011）。 该检测方案通过逐渐抑制角部响应较弱的角部靠近较强的角部，有利于图像中的均匀关键点分布。 BRISK将允许自动方向检测 - 然而，通过提取沿着投影到图像中的重力方向定向的描述符来获得更好的匹配结果。 由于IMU融合，这个方向是全球可观察的。
        作为初始化和匹配的第一步，我们使用获取的IMU测量来传播最后的姿势，以便获得状态的初步不确定估计。

    假设此时可获得一组过去的帧（包括关键帧）以及由具有足够熟知的3D位置的地标组成的局部地图（详情参见V-B）。 作为建立对应关系的第一阶段，我们执行3D-2D匹配步骤。 给定当前姿势预测，所有应该可见的地标都被考虑用于强力描述符匹配。 异常值仅在之后被拒绝。 对于可能直观地希望在引导匹配策略意义上应用逆序的读者来说，这种方案似乎是不合逻辑的; 然而，由于二进制描述符的超快速匹配，首先看图像空间一致性实际上会更昂贵。 异常值拒绝包括两个步骤：首先，我们使用不确定的姿势预测，以便在图像坐标中执行Mahalanobis测试。 其次，应用OpenGV（Kneip和Furgale，2014）中提供的绝对姿势RANSAC。

        接下来，执行2D-2D匹配以便在没有3D地标对应关系的情况下关联关键点。 同样，我们首先使用强力匹配，然后进行三角测量，以初始化地标位置，并作为拒绝异常值配对的第一步。 执行立体图像对（在非单声道情况下）的立体三角测量，以及当前帧和任何可用的先前帧之间的立体三角测量。 只有具有足够低深度不确定性的三角测量被标记为初始化 - 其余的将在后续匹配中被视为2D测量。 最后，在当前帧和最新关键帧之间执行相对RANSAC步骤（Kneip和Furgale，2014）。 相应的姿势猜测还用于最初的bootstrapping。

        图5示出了立体声情况下的典型检测和匹配结果。 请注意由于面向太阳而过度曝光的天空具有挑战性的照明。

B.关键帧选择
对于随后的优化，保持有界的一组相机帧，即在该时刻拍摄相关图像的姿势; 在这些图像*同可见的所有地标都保存在本地地图中。 如图6所示，我们区分两种帧：我们引入S最近帧的时间窗口，包括当前帧; 我们使用过去可能已经采用的许多M个关键帧。 对于关键帧选择，我们使用一个简单的启发式方法：如果投影和匹配的地标的船体覆盖的图像小于一定百分比（我们使用大约50％），或者匹配与检测到的关键点的比例很小（低于20％左右） ），框架作为关键帧插入。

VI关键帧和边缘化

与仅视觉情况相反，IMU的非线性时间约束如何能够驻留在包含可能在时间上任意远距离的关键帧的有界优化窗口中并不明显。 在下文中，我们首先提供边缘化的数学基础，即消除非线性优化中的状态，并将它们应用于视觉惯性测距。

A.非线性优化中边缘化的数学公式

高斯 - 牛顿方程组由所有误差项，雅可比矩阵和信息矩阵构成，采用Hδχ= b的形式。 让我们考虑一组被边缘化的状态，xμ，与误差项xλ相关的所有状态的集合，以及剩余状态集合xρ。 由于条件独立性，我们可以简化边缘化步骤并仅将其应用于子问题：

Schur补充操作的应用产生：

其中b*λ1和H*λ1λ1是xλ和xμ的非线性函数。

        （22）中的等式描述了边缘化的单一步骤。 在我们基于关键帧的方法中，我们必须重复应用边际化步骤，并将结果信息作为我们优化中的先验信息，同时我们的状态估计值会继续变化。 因此，我们将线性化点固定在x0附近，即边缘化时x的值。

有限偏差表示边缘化后发生的状态更新，其中x－是我们对x的当前估计。 换句话说，x组成为

（未完待续）