19TRO_Attention and Anticipation in Fast Visual-Inertial Navigation

---

论文信息

• 论文题目：Attention and Anticipation in Fast Visual-Inertial Navigation.快速视觉惯性导航中的注意力与预期
• 发表期刊：2019TRO

---

摘要

摘要——我们研究了视觉惯性导航 (VIN) 问题，其中机器人需要通过机载摄像头和惯性传感器估计其状态，而无需任何外部环境的先验知识。我们考虑了这样一种情况：由于严格的计算资源限制，机器人只能为 VIN 分配有限的资源。因此，我们回答了以下问题：在资源有限的情况下，哪些视觉线索最为相关，能够最大化 VIN 的性能？我们的方法具有四个关键要素：首先，它是任务驱动的，即视觉线索的选择是由一个量化 VIN 性能的指标所引导的。其次，它利用了“预期”概念，通过使用简化的模型进行机器人动力学的前向模拟，预测一组视觉线索在未来时间范围内的效用。第三，它高效且易于实现，因为它能够通过贪婪算法来选择最相关的视觉线索。第四，它提供了正式的性能保证：我们利用子模性理论证明，贪婪算法的选择不会远离最优（组合）选择。通过在敏捷无人机上的模拟和实际实验，我们的方法能够确保最先进的 VIN 性能，同时保持精简的处理时间。在较为简单的场景中，我们的方法在定位误差方面优于基于外观的特征选择。在最具挑战性的场景中，它能够实现准确的 VIN，而基于外观的特征选择则无法在激烈机动过程中有效跟踪机器人的运动。

索引词——空中机器人、计算机视觉、传感器融合、同步定位与地图构建 (SLAM)。

1.引言

人类大脑能够在短至 13 毫秒的时间间隔内从图像中提取概念信息[1]。人类能够在日常任务中无缝处理大量感官数据的能力有目共睹，例如在高速公路上驾驶汽车或在人群密集的街道上行走。在认知科学文献中，普遍认为，我们能够高效处理大量数据的原因在于我们能够优先处理视觉场景中的某些方面，而忽略其他信息[2]。可以想象，感官输入在争夺有限的计算资源，这些资源受大脑可用能量的限制，以及时间紧迫任务所施加的时间约束。视觉注意力是人类用来解析大量视觉数据的认知过程，它通过选择相关信息并过滤掉无关刺激，从而在有限的资源下最大化性能。

A. 机器人与人类

过去三十年，机器人技术和计算机视觉领域取得了惊人的进展，这可能促使我们提出这样的问题：机器人的感知能力与人类的表现有多大差距？我们可以通过研究不同任务的视觉处理的最新进展来探讨这个问题。我们并不声称要详尽无遗地列出所有内容，而是选择了几篇具有代表性的论文（过去三年内的研究），并且只关注它们的时间性能。一种最先进的物体检测方法[3] 在 Titan X GPU 上能够在 22 毫秒内检测场景中的物体。一种高性能的立体重建方法[4] 在单个 CPU 上（分辨率为 800 × 600）能够在 10 到 100 毫秒内构建三维（3D）场景的三角网格。一种最先进的基于视觉的同步定位与地图构建（SLAM）方法[5] 需要大约 400 毫秒进行局部地图构建和运动跟踪，且需要超过 1 秒进行全局地图优化（使用 CPU 和多个核心）。读者可能会注意到，对于每项任务，现代算法单独执行时所需的时间，都比人类解析整个场景所需的时间要长。可以说，虽然机器人技术和计算机视觉社区在推动每项任务的性能方面取得了显著进展，但我们仍然远未达到一个计算模型，在其中所有这些任务（如姿态估计、几何重建、场景理解）能够在眨眼之间并行完成。

B. 通过通用计算提高效率

有人可能会认为，赶超人类效率只是时间的问题：根据摩尔定律，计算能力以指数速度增长；因此，我们只需等待更强大的计算机即可。类似的论点可能会认为，使用 GPU 而不是 CPU 可以提高上述某些任务的性能。然而，通过与人类表现的比较，我们意识到这个论点并不完全准确。虽然我们确实可以不断增加计算资源来满足给定的时间约束（即使感官数据处理速度更快），但计算量的增加意味着能量消耗的增加；例如，Titan X GPU 的标称功耗为 250 W [6]，而 Core i7 CPU 的功耗低至 11 W [7]。另一方面，人类处理信息时始终面临有限的时间和能量约束，并且会节约地只分配完成目标所需的资源。

C. 通过专用计算提高效率

实现高速率低功耗感知，并弥合人类与机器人感知之间差距的另一种潜在解决方案是为机器感知设计专用硬件。正如我们在之前的工作[8]中广泛讨论的那样，算法和硬件的协同设计通过紧密集成算法和专用硬件，并利用应用特定集成电路和现场可编程门阵列提供的机会（如流水线、低成本运算），可以最大限度地减少资源的使用。虽然我们已经证明，使用专用硬件进行视觉惯性导航（VIN）能够将功耗降低1到2个数量级（同时保持相当的性能），但有三个主要观察结论促使了本研究。首先，开发用于感知的专用硬件是一个昂贵且耗时的过程，而且生成的硬件很难进行升级。其次，与其设计出能够满足特定性能要求的优化硬件，不如开发一个能够系统性地在性能与计算之间进行权衡的框架，从而更灵活地适应可用的、可能随时间变化的计算资源和性能需求。第三，大量生物学证据表明，有效的感知既需要专门的电路（例如，人类的视觉感知是由大脑中高度专门化的区域执行的[9]），也需要一种优先处理刺激的机制（即视觉注意力[2]）。

D. 贡献

在本文中，我们研究了如何通过以任务相关的方式对传感器数据进行优先排序，从而加速视觉导航中的计算（或等效地减少计算工作量）。具体来说，我们专注于运动估计任务（VIN），并考虑在机载计算资源有限的情况下，机器人只能使用环境中少量的视觉特征来支持运动估计的情况。接着，我们设计了一种视觉注意力机制，选择一组合适的视觉特征以最大化定位精度；我们的一般框架将在第 III 部分中介绍。

我们的方法是任务驱动的：我们考虑的是运动估计任务（VIN），我们的算法选择能最大化任务相关性能指标的特征，这些指标将在 III-A 部分中介绍。与现有的视觉特征选择文献不同，我们认为特征的效用并非特征本身的固有属性（例如外观），而是源于环境与观察者状态的相互作用。我们的方法无缝地捕捉了特征的视觉显著性和任务相关的效用。

我们的注意力机制本质上是预测性的：在决定哪个特征更有用时，我们的方法通过快速前向模拟机器人状态来选择特征，从而使得特征选择能够反映机器人的动态变化和“意图”。前向模拟基于一个简化模型，我们将在 III-E 节中介绍该模型。

由于资源的最优分配是一个困难的组合优化问题，在第四部分中，我们提出了一种用于注意力分配的贪婪算法。在同一部分中，我们利用子模性理论的最新成果，为贪婪算法提供了正式的性能保证。第四部分还回顾了基于凸松弛的相关技术。

第五部分对所提出的方法进行了实验评估。结果表明，我们的方法能够提升标准 VIN 流程中的性能，并在敏捷运动和严格的资源约束下实现准确的导航。所提出的方法在很大程度上优于基于外观的特征选择方法，并显著减少了 VIN 后端所需的计算时间。

本文扩展了 [10] 中提出的初步结果。特别是，关于特征选择的凸松弛方法的讨论（见第 IV-A 和 IV-B 节）、命题 12 的性能保证、第 V-A 节的模拟结果，以及对 11 个 EuRoC 数据集 [11] 的实验评估，都是全新的内容，之前未曾公开发表。

II. 相关工作

本研究涉及多个领域的相关工作。

A. 神经科学和心理学中的注意力与显著性

注意力是人类和动物视觉研究的核心主题，自1980年代以来已发表超过2500篇相关论文[2]。

虽然完整的介绍超出了本文的范围，但我们回顾了几个基本概念，主要参考了Carrasco [2]、Borji和Itti [12]、Scholl [13]以及Caduff和Timpf [14]的工作。Scholl [13] 将注意力定义为对感官刺激的辨别，以及将有限资源分配给相互竞争的注意力需求。Carrasco [2] 确定了三种类型的注意力：空间注意力、基于特征的注意力和基于对象的注意力。空间注意力通过将眼睛移向特定位置（显性注意）或聚焦于视野中的相关位置（隐性注意）来优先关注场景的不同位置。基于特征的注意力优先检测特定特征（如颜色、运动方向、朝向），无论其位置如何。基于对象的注意力则优先处理特定的物体。在本研究中，我们主要关注隐性空间注意力：即视野中哪些位置对导航最为重要？人类的隐性注意力是自愿与非自愿机制的结合，能够引导在特定位置对视觉刺激的处理[2]。实证研究表明，灵长类动物和人类的注意力是与任务相关的[14][15]。Borji和Itti [12] 明确区分了自下而上与自上而下的注意力模型；在自下而上的模型中，注意力由视觉线索（刺激驱动）捕获，而在自上而下的模型中，注意力由观察者的目标引导。Caduff和Timpf [14] 研究了人类导航中的地标显著性，得出结论，显著性源于地标的内在属性（如外观）与观察者的状态（如先验知识、观察姿势）的交织。另一个重要方面可追溯到Wolfe [16] 和Spekreijse [17]的引导搜索理论，即前注意过程与注意过程之间的区别。前注意过程并行处理所有传入的感官数据；而注意过程则只处理大脑认为更相关的经过筛选的数据。

B. 机器人与计算机视觉中的特征选择

通过主动特征选择来增强视觉SLAM和视觉里程计的性能并不新颖。Sim和Dudek [18] 以及Peretroukhin等人 [19] 使用训练数据学习视觉特征质量模型。每个特征都从手工构建的预测空间映射到一个标量权重，该权重量化了其在姿势估计中的有用性；在[19]中，这些权重随后被用来重新调整每个观测值的测量协方差。Ouerhani等人 [20] 使用注意力地标构建了拓扑地图。Newman和Ho [21] 考虑了一种配备摄像头和激光测距仪的机器人，并通过基于外观的视觉显著性概念进行特征选择。Sala等人 [22] 使用共可见性标准来选择从多个视点均可见的良好地标。Siagian和Itti [23] 研究了蒙特卡洛定位中的生物启发式注意模型。Frintrop和Jensfelt [24] 使用注意力框架进行地标选择和主动注视控制；特征选择基于VOCUS模型 [24]，该模型包括自下而上的注意系统（根据特征外观计算显著性），并可结合自上而下的机制（考虑任务性能）。而主动注视控制则通过三种行为的组合来实现：地标重新检测、地标追踪和新区域探索。Hochdorfer和Schlegel [25] 提出了一种基于区域覆盖的地标评级与选择机制，以实现终身映射。Strasdat等人 [26] 提出了一种强化学习方法用于地标选择。Chli和Davison [27] 以及Handa等人 [28] 使用可用的先验信息来指导特征匹配，从而降低计算成本。Jang等人 [29] 提出了一种地标分类方法，以提高视觉里程计的准确性；每个特征类别分别用于估计自我运动的旋转和平移分量。Shi等人 [30] 提出了一种特征选择技术，以提高SLAM中数据关联的鲁棒性。视觉注意和显著性概念也在场景理解的研究中得到了探讨。例如，Oliva和Torralba [31] 提出了“空间包络”概念，用于获得场景的粗略描述，从而抽象出不必要的细节；而Torralba等人 [32] 提出了一种用于自然搜索任务的注意机制，该机制结合了自下而上的显著性和自上而下的因素，以识别可能吸引人类观察者注意的图像区域，进而寻找给定的物体。计算机视觉领域最近的研究开始利用注意力机制来减少神经网络的计算负担。Mnih等人 [33] 通过引入“glimpse”概念，减少了使用循环神经网络进行物体检测和跟踪的计算量，从而为图像中的感兴趣区域提供了更高的分辨率。Xu等人 [34] 使用视觉注意力来改进图像内容的描述。Cvisić和Petrovic [35] 通过特征选择加速了立体里程计的计算；选择过程基于分桶方法（将特征均匀分布在图像中）以及基于外观的排名。

我们的方法与图稀疏化技术有一定关联，后者通过从SLAM因子图中后验地修剪特征来减少计算量；我们请读者参考[36]中的综述，以了解这些技术的详细信息。

与我们的提议最相关的贡献是Davison [37]、Lerner等人 [38]、Mu等人 [39]、Wu等人 [40] 以及Zhang和Vela [41]的研究。Davison [37] 的开创性工作是第一批使用信息论结构来推理视觉特征的论文之一，且与本文讨论的许多动机相符。与本文不同，Davison [37] 考虑了一个基于模型的跟踪问题，其中需要根据已知特征的观察来估计移动相机的状态。回顾来看，我们也为使用贪婪算法（类似于[37]中的方法）提供了理论依据，并证明该算法能够计算出接近最优的解。Lerner等人 [38] 研究了已知地标的定位问题中的地标选择，机器人需要选择一部分地标进行观察，以最小化定位不确定性。最优子集选择被建模为一个混合整数规划问题，并放松为SDP。尽管我们在本文中解决的问题（视觉惯性里程计与已知地图的定位）有所不同，然而[38]中的一个有趣方面是使用了需求矩阵，该矩阵对状态协方差加权并编码任务相关的不确定性约束。Mu等人 [39] 提出了一种两阶段方法，选择地标子集以最小化碰撞概率，同时选择测量子集以准确定位这些地标。我们的方法共享任务驱动的测量选择理念，但有三个主要区别。首先，我们使用简化的线性模型进行前向动力学模拟：这一做法与RANSAC的精神相符，因为简化的代数模型用于快速过滤掉不太相关的数据。其次，我们考虑了不同的性能指标，超越了[39]及相关图稀疏化工作中使用的行列式标准。第三，我们在单阶段中执行特征选择，并利用子模性理论提供了正式的性能保证。Wu等人 [40] 考虑了一个多摄像机系统，将特征选择过程分为两个资源分配问题：1）如何在各摄像机间分配资源，2）如何根据分配的资源在每个摄像机中选择特征。前一个问题通过对特征分布做出简化假设来解决，后一个问题则基于[42]的启发式特征选择方案。我们的论文试图通过利用子模性的概念来形式化特征选择。Zhang和Vela [41] 使用可观测性分数进行特征选择，并通过子模性理论提供次优性保证。我们的提议在思路上与[41]相似，但有几个重要区别。首先，我们的方法基于预期：特征选择器能够感知机器人的意图（未来运动），并据此选择特征。其次，我们在固定滞后平滑的框架下进行操作，并研究了其他性能指标。第三，从理论角度出发，我们提供了乘法次优性界限，并证明了这些界限在某些条件下不会消失。

C. 传感器调度与子模块性

特征选择与控制理论中的传感器调度问题密切相关。传感器调度的最常见设置是：N个传感器监测一个感兴趣的现象，必须从N个可用传感器中选择κ个，以最大化某些信息收集指标；这一设置也被称为传感器选择或传感器布置。传感器选择的文献中包括基于凸松弛[43]、贝叶斯最优设计[44]和子模块优化[45]的方法。该问题在[46]中被证明是NP难的。Shamaiah等人[47]利用子模块性，并在优化估计误差协方差的对数行列式时提供了性能保证。与本文中更为接近的设置是传感器监测的现象是动态的；在这种情况下，传感器调度可以通过选择N个可能测量值中的κ个来表述，用于卡尔曼滤波器（KF）更新阶段。Vitus等人[48]使用树搜索方法进行传感器调度。Zhang等人[49]证明了卡尔曼滤波中的传感器调度是NP难的，并且证明了卡尔曼滤波稳态先验和后验协方差的迹不是子模块的，尽管在实践中贪婪算法表现良好。Jawaid和Smith[50]提供了反例，表明一般来说，协方差的最大特征值和迹不是子模块的。Tzoumas等人[51]推广了[50]中的推导，在对观察矩阵做出某些假设下，证明了固定时间范围内对数行列式的子模块性。Summers等人[52]展示了基于可控性和可观察性Gramians的几个度量是子模块的。

D. 视觉惯性导航

由于视觉系统与前庭系统的结合使用是人类导航的关键，移动机器人中视觉惯性导航（VIN）的最新进展使得在无 GPS 环境中，使用商用硬件实现前所未有的姿态估计性能成为可能。关于VIN的文献非常广泛，过去两年中提出了许多贡献，包括基于滤波的方法[42]、[53]–[57]、固定滞后平滑[58]–[61]和完全平滑[62]–[68]。我们请读者参阅[68]以获取全面的回顾。

E. 符号

我们分别用小写和大写粗体字母表示向量（例如 v）和矩阵（例如 M）。集合用无衬线字体表示（例如 A）。非粗体字母用于标量和索引（例如 j）以及函数名称（例如 f(·)）。符号 |A| 表示集合 A 的基数。大小为 n 的单位矩阵用 In 表示。m × n 零矩阵用 0m×n 表示。M ≽ 0 表示矩阵 M 是半正定的。符号 ‖·‖ 表示向量的欧几里得范数和矩阵的谱范数。

III. VIN 中的注意力机制

我们设计了一种注意力机制，从当前帧中选择 κ 个相关的视觉特征（例如 Harris 角点），以最大化视觉惯性运动估计的性能。这些 κ 个特征必须从摄像机图像中的 N 个可用特征中选择；该方法可以处理单目摄像机和立体摄像机（立体摄像机视为一对刚性单目摄像机）。

我们称 $\mathbf{F}$ 为所有可用特征的集合（其中 $|\mathbf{F}|=N$）。如果用 $f(\cdot )$ 表示我们任务相关的性能度量（我们将在第III-A节中为VIN正式定义一个合适的度量），那么我们的特征选择问题可以表述如下：
$$\underset{S\subset \mathbf{F}}{\max }f(S)\text{subject to}|S|\le \kappa .\text{\hspace{1em}(1)}$$

该问题寻找一个特征子集 $S$，其中包含不超过 $\kappa$ 个特征，以优化任务性能 $f(\cdot )$。这是一个标准的特征选择问题，已广泛应用于多个领域，包括机器学习 [69]、机器人技术 [39] 和传感器网络 [43]。问题 (1) 通常是 NP 难的 [46]。在本文的其余部分中，我们致力于为我们的 VIN 任务设计一个合适的性能指标 $f(S)$，并提供快速的近似算法来解决问题 (1)。

我们希望设计一个性能指标 $f(\cdot )$，用于捕捉任务相关的要求：在我们的例子中，该指标必须量化VIN运动估计中的不确定性。此外，该指标还应捕捉相关研究中被视为重要的方面。首先，该指标必须奖励选择最具区分性的特征（就外观而言），因为这些特征更有可能在连续的帧中被重新观测到。其次，该指标必须奖励那些在视野内停留时间更长的特征。因此，\textbf{预期} 是一个关键方面：该指标必须能够意识到在某些运动条件下，某些特征更可能保留在摄像机的视野内。第三，该指标必须奖励那些更具信息量的特征，从而减少不确定性。在下一节中，我们提出了两个能够无缝捕捉所有这些方面的性能指标。

A. 基于任务的VIN性能指标

这里我们提出了两个性能指标，用于量化在选择一组视觉特征 $S$ 的情况下，视界 $H$ 内估计误差的累积。假设 $k$ 是需要选择特征的时间瞬间。我们将 ${\hat{x}}_k$ 称为机器人在时间 $k$ 的（待计算的）状态估计：关于 ${\hat{x}}_k$ 中所包含的变量，我们将在第III-E1节中更精确地说明。现在，读者可以将 ${\hat{x}}_k$ 理解为包含机器人在时间 $k$ 的姿态、速度估计，以及惯性测量单元（IMU）的偏差。

我们用 ${\hat{x}}_{k:k+H}$ 表示视界 $H$ 内的未来状态估计，其中
$${\hat{x}}_{k:k+H}\triangleq [{\hat{x}}_k,{\hat{x}}_{k+1},\dots ,{\hat{x}}_{k+H}].$$

此外，我们将 $P_{k:k+H}$ 称为估计 ${\hat{x}}_{k:k+H}$ 的协方差矩阵，并将
$${\Omega }_{k:k+H}\triangleq P_{k:k+H}^{-1}$$

称为对应的信息矩阵。下面将描述两个用于衡量 ${\hat{x}}_{k:k+H}$ 准确度的自然性能指标。

B. 最坏情况估计误差

最坏情况误差方差由协方差矩阵 $P_{k:k+H}$ 的最大特征值 ${\lambda }_{\text{max}}(P_{k:k+H})$ 进行量化，参见 [43]。将 ${\lambda }_{\text{min}}({\Omega }_{k:k+H})$ 定义为信息矩阵 ${\Omega }_{k:k+H}$ 的最小特征值，则有
$${\lambda }_{\text{max}}(P_{k:k+H})=\frac{1}{{\lambda }_{\text{min}}({\Omega }_{k:k+H})},$$
因此，最小化最坏情况误差等价于最大化 ${\lambda }_{\text{min}}({\Omega }_{k:k+H})$。

注意，信息矩阵 ${\Omega }_{k:k+H}$是所选测量集 $S$ 的函数，因此我们将其表示为 ${\lambda }_{\text{min}}({\Omega }_{k:k+H}(S))$。

因此，我们的第一个指标（要最大化）为：
$$f_{\lambda }(S)={\lambda }_{\text{min}}({\Omega }_{k:k+H}(S))={\lambda }_{\text{min}}\left({\bar{\Omega }}_{k:k+H}+\sum _{l\in S}{\Delta }_l\right),\text{\hspace{1em}(2)}$$
其中在右侧，我们利用了信息矩阵的加法结构：${\bar{\Omega }}_{k:k+H}$ 是未选择任何特征时估计的信息矩阵（直观上，这是由IMU积分产生的协方差的逆矩阵），而 ${\Delta }_l$ 是与选择第 $l$ 个特征相关联的信息矩阵。我们将在第III-E节中给出 ${\bar{\Omega }}_{k:k+H}$ 和 ${\Delta }_l$ 的明确表达式。

C. 置信椭球的体积和平均半径

$\epsilon$-置信椭球是包含估计误差概率为 $\epsilon$ 的椭球。置信椭球的体积和平均半径与协方差矩阵的行列式成正比。具体来说，与协方差矩阵 $ P_{k:k+H} $ 相关的 $n$-维椭球的体积 $ V $ 和平均半径 $\bar{R}$ 可以表示为 [43]：
$$V=\frac{(\alpha \pi {)}^{n/2}}{\Gamma \left(\frac{n}{2}+1\right)}\det \left(P_{k:k+H}^{\frac{1}{2}}\right),\bar{R}=\sqrt{\alpha }\det {\left(P_{k:k+H}\right)}^{\frac{1}{2n}},\text{\hspace{1em}(3)}$$
其中，$\alpha$ 是自由度为 $n$ 且上尾概率为 $\epsilon$ 的 ${\chi }^2$ 分布的分位数，$\Gamma (\cdot )$ 是伽马函数，$\det (\cdot )$ 是方阵的行列式。

从(3)式中我们可以看到，为了最小化置信椭球的体积和平均半径，我们可以等效地最小化协方差矩阵的行列式。此外，由于：
$$\log \det \left(P_{k:k+H}\right)=\log \det \left({\Omega }_{k:k+H}^{-1}\right)=-\log \det \left({\Omega }_{k:k+H}\right),$$
因此，最小化置信椭球的大小等价于最大化信息矩阵的对数行列式，从而得到我们的第二个性能指标：
$$f_{\det }(S)=\log \det \left({\Omega }_{k:k+H}(S)\right)=\log \det \left({\bar{\Omega }}_{k:k+H}+\sum _{l\in S}{\Delta }_l\right),\text{\hspace{1em}(4)}$$
其中我们再次指出，信息矩阵是所选特征的函数，可以写成加法形式。

D. 概率特征轨迹

到目前为止所描述的性能指标已经捕捉到一些重要的方面：它们是任务相关的，因为它们不仅量化了运动估计的性能；此外，它们还具有预测性，因为它们着眼于在短期（未来）内选择一组特征的效果。正如我们将在 III-E2 节中看到的那样，该模型还捕捉到较长的特征轨迹更具信息量这一事实，因此它隐式地鼓励选择能够在多个帧之间共视的特征。唯一尚未建模的方面是，即使一个特征位于摄像机的视野范围内，它仍然有可能无法被正确跟踪，从而导致相应的特征轨迹丢失。例如，如果某个特征的外观不够具有辨识度，则其特征轨迹可能会比预期的更短。

为了对特征跟踪丢失的概率进行建模，我们引入 $N$ 个伯努利随机变量 $b_1,\dots ,b_N$。每个变量 $b_l$ 表示特征 $l$ 的跟踪结果：若 $b_l=1$，则表示该特征被成功跟踪；否则，特征跟踪丢失。对于每个特征，我们假设给定 $p_l=\text{Prob}(b_l=1)$。实际上，可以将每个特征的外观与 $p_l$ 相关联，从而使得外观更具辨识度的特征具有更高的被跟踪概率，或者可以通过数据学习这些概率。使用二进制变量 b = { b 1 , … , b N } \mathbf{b} = \{b_1, \dots, b_N\} b={b1​,…,bN​}，我们将视界末尾的信息矩阵表示为：
$${\Omega }_{k:k+H}(\mathbf{S},\mathbf{b})={\bar{\Omega }}_{k:k+H}+\sum _{l\in \mathbf{S}}{b}_l{\Delta }_l\text{\hspace{1em}(5)}$$

这有一个清晰的解释：如果第 $l$ 个特征被正确跟踪，则 $b_l=1$，相应的信息矩阵 ${\Delta }_l$ 将被添加到 ${\bar{\Omega }}_{k:k+H}$ 中；另一方面，如果特征跟踪丢失，则 $b_l=0$，相应的信息内容将从 (5) 中的总和中消失。

由于 $\mathbf{b}$ 是一个随机向量，我们的信息矩阵 ${\Omega }_{k:k+H}(\mathbf{S},\mathbf{b})$ 现在是一个随机量，因此我们必须重新定义性能指标，以包括在 $\mathbf{b}$ 上的期望值：
$$f(\mathbf{S})=\mathbb{E}[f({\Omega }_{k:k+H}(\mathbf{S},\mathbf{b}))]\text{\hspace{1em}(6)}$$
其中，函数 $f(\cdot )$ 表示 $f_{\lambda }(\cdot )$ 或 $f_{\text{det}}(\cdot )$。

直接计算期望式 $(6)$ 会导致含有组合数量项的求和，难以有效评估。为了避免组合爆炸问题，我们使用 \textbf{Jensen 不等式}，该不等式对凹函数的期望提供一个上界，如下所示：
$$\mathbb{E}[f({\Omega }_{k:k+H}(\mathbf{S},\mathbf{b}))]\le f(\mathbb{E}[{\Omega }_{k:k+H}(\mathbf{S},\mathbf{b})]).\text{\hspace{1em}(7)}$$

由于 $f_{\lambda }(\cdot )$ 和 $f_{\text{det}}(\cdot )$ 都是凹函数，\textbf{Jensen 不等式} 为我们的预期成本提供了一个上界。在本文的其余部分中，我们最大化这个界限，而不是原始成本。这么做的好处是，(7) 式的右侧可以被高效地计算为：
$$f(\mathbb{E}[{\Omega }_{k:k+H}(\mathbf{S},\mathbf{b})])=f\left({\bar{\Omega }}_{k:k+H}+\sum _{l\in \mathbf{S}}{p}_l{\Delta }_l\right).\text{\hspace{1em}(8)}$$

其中，我们使用了定义 (5)，期望是一个线性算子，以及 $\mathbb{E}[b_l]=p_l$ 这一事实。因此，我们的性能指标可以明确地写成：
$$f_{\lambda }(\mathbf{S})={\lambda }_{\text{min}}\left({\bar{\Omega }}_{k:k+H}+\sum _{l\in \mathbf{S}}{p}_l{\Delta }_l\right),$$
$$f_{\text{det}}(\mathbf{S})=\log \det \left({\bar{\Omega }}_{k:k+H}+\sum _{l\in \mathbf{S}}{p}_l{\Delta }_l\right).\text{\hspace{1em}(9)}$$

当 $p_l=1,\forall l$ 时，它与确定性对应项 (2)、(4) 相一致。有趣的是，在 (9) 中，特征未被跟踪的概率会直接折扣其对应的信息内容。因此，该方法将更有可能丢失的特征视为信息量较少的特征，这是一种期望的行为。我们需要注意，概率 $p_l$ 仅捕捉视觉特征的独特性，而几何特性（例如，共可见性）则体现在矩阵 ${\Delta }_l$ 中，如第 III-E2 节所述。虽然到目前为止的推导非常通用，并为任何基于特征的 SLAM 系统提供了特征选择机制，但在接下来的内容中，我们将重点讨论视觉惯性导航 (VIN)，并为公式 (9) 中出现的矩阵 ${\bar{\Omega }}_{k:k+H}$ 和 ${\Delta }_l$ 提供显式表达式。

E. 前向模拟模型

第三部分中提出的特征选择模型和第三部分 A 中的指标需要预测信息矩阵在视界 $H$ 上的演变。接下来，我们将展示如何对 IMU 和相机进行前向模拟；需要注意的是，我们不需要模拟实际的 IMU 测量值，而只需预测对应的信息矩阵，该矩阵依赖于 IMU 的噪声统计数据。

前向模拟模型依赖于机器人的未来运动（IMU 和视觉模型都是未来机器人姿态的函数）；因此，\textbf{预测} 是我们方法的一个关键元素：特征选择机制能够感知机器人在短期内的运动意图，并相应地选择特征。正如我们将在实验中看到的，这使得系统能够在急转弯和激进操作期间更智能地选择特征。在实际应用中，视界内的未来姿态可以通过当前的控制动作来计算；例如，如果控制器在回退视界（receding horizon）上进行规划，则可以通过积分车辆的动态模型来获得未来姿态。因此，我们的注意力机制实现了控制与感知的紧密集成。

我们在第四部分中介绍的特征选择算法是通用的，适用于任何正定矩阵 ${\bar{\Omega }}_{k:k+H}$ 和任何半正定矩阵 ${\Delta }_l$。因此，不感兴趣的读者可以放心地跳过本节，该节提供了在 VIN 设置中 ${\bar{\Omega }}_{k:k+H}$ 和 ${\Delta }_l$ 的明确表达式。

在深入研究 IMU 和视觉模型的细节之前，我们先强调前向模拟模型的一个关键设计目标：\textbf{效率}。注意力机制的目标是减少处理管道后期的认知负荷；因此，从设计上讲，它不应对计算要求过高，否则将违背其初衷。基于此原因，在本节中，我们提出了一个简化的 VIN 模型，该模型旨在计算高效，同时捕捉完整视觉惯性估计管道中所有重要的方面，例如 \cite{68}。

1. IMU 模型和先验:我们的简化 IMU 模型基于一个假设：由于陀螺仪积分在时间范围内累积的旋转误差可以忽略不计。换句话说，陀螺仪预测的相对旋转估计是准确的。即使对于廉价的 IMU，这个假设也是现实的：旋转积分的漂移通常很小，在我们注意力系统所考虑的时间范围内可以忽略不计（在我们的测试中，我们考虑的是 3 秒的时间范围）。

假设旋转准确已知，则可以将状态限制为机器人的位置、线速度和加速度计偏差。因此，在本文的其余部分，机器人在时间 $k$ 的（未知）状态表示为：
$
\mathbf{x}_k = [\mathbf{t}_k, \mathbf{v}_k, \mathbf{b}_k],
$
其中 ${\mathbf{t}}_k\in {\mathbb{R}}^3$ 是机器人的三维位置，${\mathbf{v}}_k\in {\mathbb{R}}^3$ 是其速度，${\mathbf{b}}_k$ 是随时间变化的加速度计偏差。我们还使用符号 ${\mathbf{R}}_k$ 表示机器人在时间 $k$ 的姿态：假设该姿态可以通过地平线 $H$ 内的陀螺仪积分得到，因此它不属于状态变量的一部分。

与大多数视觉惯性导航 (VIN) 管道类似，我们希望估计机器人在每一帧的状态。因此，本节的目标与 \cite{68} 类似，是将两个连续帧 $k$ 和 $j$ 之间的一组 IMU 测量值重新表述为约束 ${\mathbf{x}}_k$ 和 ${\mathbf{x}}_j$ 的单个测量值。与 \cite{68} 不同的是，我们展示了如何获得\textbf{线性测量模型}。

机载加速度计测量传感器相对于惯性系的加速度 ${\mathbf{a}}_k$，并受到加性白噪声 ${\eta }_k$ 和缓慢变化的传感器偏差 ${\mathbf{b}}_k$ 的影响。因此，加速度计在时间 $k$ 获取的测量值 ${\tilde{\mathbf{a}}}_k\in {\mathbb{R}}^3$ 建模为 \cite{68}：

$${\tilde{\mathbf{a}}}_k={\mathbf{R}}_k^{\top }({\mathbf{a}}_k-\mathbf{g})+{\mathbf{b}}_k+{\eta }_k,\text{\hspace{1em}(10)}$$

其中，$\mathbf{g}$ 是重力矢量，在惯性系中表示。为了简化符号表示，我们在符号中省略了参考坐标系，遵循了 \cite{68} 中使用的惯例：位置 ${\mathbf{t}}_k$ 和速度 ${\mathbf{v}}_k$ 在（惯性）世界坐标系中表示，而偏差 ${\mathbf{b}}_k$ 则在传感器坐标系中表示。

给定时刻 $k$ 的位置 ${\mathbf{t}}_k$ 和速度 ${\mathbf{v}}_k$，我们可以进行前向积分，得到时刻 $j>k$ 的位置 ${\mathbf{t}}_j$ 和速度 ${\mathbf{v}}_j$：

$${\mathbf{v}}_j={\mathbf{v}}_k+\sum _{i=k}^{j-1}{\mathbf{a}}_i\delta$$

从方程 (10) 中我们知道：

$${\mathbf{a}}_i=\mathbf{g}+{\mathbf{R}}_i({\tilde{\mathbf{a}}}_i-{\mathbf{b}}_i-{\mathbf{\eta }}_i),$$

并假设帧间偏差为常数，即 ${\mathbf{b}}_i={\mathbf{b}}_k$，代入上式可得：

$${\mathbf{v}}_j={\mathbf{v}}_k+\mathbf{g}{\delta }_{kj}+\sum _{i=k}^{j-1}{\mathbf{R}}_i({\tilde{\mathbf{a}}}_i-{\mathbf{b}}_k-{\mathbf{\eta }}_i)\delta .\text{\hspace{1em}(11)}$$

位置 ${\mathbf{t}}_j$ 可由以下积分表示：

$${\mathbf{t}}_j={\mathbf{t}}_k+\sum _{i=k}^{j-1}\left({\mathbf{v}}_i\delta +\frac{1}{2}{\mathbf{a}}_i{\delta }^2\right).$$

将 ${\mathbf{a}}_i=\mathbf{g}+{\mathbf{R}}_i({\tilde{\mathbf{a}}}_i-{\mathbf{b}}_k-{\mathbf{\eta }}_i)$ 代入上式，得到：

$${\mathbf{t}}_j={\mathbf{t}}_k+\sum _{i=k}^{j-1}\left({\mathbf{v}}_i\delta +\frac{1}{2}\mathbf{g}{\delta }^2+\frac{1}{2}{\mathbf{R}}_i({\tilde{\mathbf{a}}}_i-{\mathbf{b}}_k-{\mathbf{\eta }}_i){\delta }^2\right).$$

将 ${\mathbf{v}}_j$ (由方程 (11)) 代入，令 $j=i$，最终得到：

$${\mathbf{t}}_j={\mathbf{t}}_k+\frac{1}{2}\mathbf{g}{\hat{\delta }}_{kj}^2+\sum _{i=k}^{j-1}\frac{1}{2}{\mathbf{R}}_i({\tilde{\mathbf{a}}}_i-{\mathbf{b}}_k-{\mathbf{\eta }}_i){\delta }^2+\sum _{i=k}^{j-1}\left({\mathbf{v}}_k+\mathbf{g}{\delta }_{ki}+\sum _{h=k}^{i-1}{\mathbf{R}}_h({\tilde{\mathbf{a}}}_h-{\mathbf{b}}_k-{\mathbf{\eta }}_h)\delta \right).\text{\hspace{1em}(12)}$$

其中 $\delta$ 是 IMU 的采样时间，${\delta }_{kj}={\sum }_{i=k}^{j-1}\delta$，${\hat{\delta }}_{kj}^2={\sum }_{i=k}^{j-1}{\delta }^2$。与 \cite{68} 类似，我们假设 IMU 偏差在两帧之间保持不变。跨帧偏差的演变可以建模为随机游走：

$${\mathbf{b}}_j={\mathbf{b}}_k-{\mathbf{\eta }}_{kj}^b,\text{\hspace{1em}(13)}$$

其中 ${\mathbf{\eta }}_{kj}^b$ 是零均值随机向量。

注意到状态在方程 (11)-(13) 中呈线性形式，因此可以将这三个表达式重写为矩阵形式：

$${\mathbf{z}}_{kj}^{\text{IMU}}={\mathbf{A}}_{kj}{\mathbf{x}}_{k:k+H}+{\mathbf{\eta }}_{kj}^{\text{IMU}}.\text{\hspace{1em}(14)}$$

其中 ${\mathbf{z}}_{kj}^{\text{IMU}}\in {\mathbb{R}}^9$ 是一个合适的向量，${\mathbf{\eta }}_{kj}^{\text{IMU}}\in {\mathbb{R}}^9$ 是零均值随机噪声。我们注意到，虽然 ${\mathbf{z}}_{kj}^{\text{IMU}}$ 是未来 IMU 测量值的函数，但在我们的方法中实际上并未使用该向量（重要的是矩阵 ${\mathbf{A}}_{kj}$ 和噪声 ${\mathbf{\eta }}_{kj}^{\text{IMU}}$ 的信息矩阵），因此不需要模拟未来的测量值。附录 A 给出了矩阵 ${\mathbf{A}}_{kj}\in {\mathbb{R}}^{9\times 9(H+1)}$、向量 ${\mathbf{z}}_{kj}^{\text{IMU}}$ 和噪声 ${\mathbf{\eta }}_{kj}^{\text{IMU}}$ 协方差的明确表达式。${\mathbf{A}}_{kj}$ 是一个稀疏块矩阵，包含 $9\times 9$ 的块结构，除了对应于时间 $k$ 和 $j$ 状态的块外，其余均为零。

根据线性估计理论，我们知道，利用视界 $H$ 中所有连续帧 $k,j$ 的 IMU 测量值 $(14)$，给定 IMU 数据，状态 ${\mathbf{x}}_{k:k+H}$ 的最佳估计的信息矩阵可表示为：

$${\bar{\Omega }}_{k:k+H}^{\text{IMU}}=\sum _{k,j\in H}\left({\mathbf{A}}_{kj}^{\top }{\Omega }_{kj}^{\text{IMU}}{\mathbf{A}}_{kj}\right).\text{\hspace{1em}(15)}$$

其中 $H$ 是时间范围 $H$ 内的连续帧集，${\Omega }_{kj}^{\text{IMU}}\in {\mathbb{R}}^{9\times 9}$ 是在(14)中引入的噪声向量 ${\eta }_{kj}^{\text{IMU}}$ 的信息矩阵。

虽然 IMU 测量值限制了未来时间范围 $H$ 中的状态，但时间 $k+H$ 处的预测信息矩阵也会受到时间 $k$ 处初始信息矩阵的影响。该信息矩阵称为 ${\bar{\Omega }}_k^{\text{PRIOR}}\in {\mathbb{R}}^{9\times 9}$，由 VIN 估计器计算和维护，可以理解为时间 $k$ 处状态的先验信息。该信息矩阵的存在导致在表达式 ${\bar{\Omega }}_{k:k+H}$ 中出现一个附加项，${\bar{\Omega }}_{k:k+H}$ 是在选择任何视觉测量之前的状态估计信息矩阵，如式(5)所示。具体而言，${\bar{\Omega }}_{k:k+H}$ 可以写成：

$${\bar{\Omega }}_{k:k+H}={\bar{\Omega }}_{k:k+H}^{\text{IMU}}+{\bar{\Omega }}_{k:k+H}^{\text{PRIOR}}\text{\hspace{1em}(16)}$$
其中 ${\bar{\Omega }}_{k:k+H}^{\text{PRIOR}}$ 是一个矩阵，除了左上角的 $9\times 9$ 块外，其余部分均为零，该块等于 ${\bar{\Omega }}_k^{\text{PRIOR}}$，以反映我们对时间 $k$ 状态的先验信息。矩阵 ${\bar{\Omega }}_{k:k+H}$ 表示在选择视觉测量之前状态 ${\mathbf{x}}_{k:k+H}$ 的最佳估计的信息矩阵，并遵循我们在(5)中已介绍的符号。先验和测量的存在与标准固定滞后平滑器完全类似，而这里我们利用了线性模型的优势。

2. 视觉模型: 同样，对于视觉测量，我们旨在设计一个线性测量模型，以简化实际的（非线性）透视投影模型。为此，我们必须将像素测量表示为我们所需估计的未知状态的线性函数。

外部 $3$-D 点（或地标）$l$ 的（校准）像素测量识别了相机坐标系中该地标的 $3$-D 方位。从数学上讲，如果我们将 ${\mathbf{u}}_{kl}$ 称为与机器人在时间 $k$ 时的位姿观测到地标 $l$ 所对应的单位向量，则 ${\mathbf{u}}_{kl}$ 满足以下关系：
$${\mathbf{u}}_{kl}\times \left({\left({\mathbf{R}}_{\text{cam},k}^W\right)}^T({\mathbf{p}}_l-{\mathbf{t}}_{\text{cam},k}^W)\right)=0_3\text{\hspace{1em}(17)}$$
其中，$\times$ 表示两个向量的叉积，${\mathbf{p}}_l$ 是地标 $l$ 在世界坐标系中的三维位置，${\mathbf{R}}_{\text{cam},k}^W$ 和 ${\mathbf{t}}_{\text{cam},k}^W$ 分别表示时间 $k$ 时相机相对于世界坐标系的旋转和平移。换句话说，模型 (17) 要求观测到的点（变换到相机坐标系中）与测量方向 ${\mathbf{u}}_{kl}$ 共线，因为叉积衡量了与共线性的偏差 \cite{71}。

现在我们注意到，对于两个向量 ${\mathbf{v}}_1$ 和 ${\mathbf{v}}_2$，叉积 ${\mathbf{v}}_1\times {\mathbf{v}}_2$ 可以表示为 ${\left[{\mathbf{v}}_1\right]}_{\times }{\mathbf{v}}_2$，其中 ${\left[{\mathbf{v}}_1\right]}_{\times }$ 是由 ${\mathbf{v}}_1$ 构建的斜对称矩阵。此外，我们注意到相机相对于世界坐标系的姿态 $({\mathbf{R}}_{\text{cam},k}^W,{\mathbf{t}}_{\text{cam},k}^W)$ 可以表示为 IMU 相对于世界坐标系的姿态 $({\mathbf{R}}_k,{\mathbf{t}}_k)$ 和相机相对于 IMU 的相对姿态 $({\mathbf{R}}_{\text{cam}}^{\text{IMU}},{\mathbf{t}}_{\text{cam}}^{\text{IMU}})$（已通过校准获得）的组合。基于这两个考虑，我们将式(17) 等效地重写为：
$${\left[{\mathbf{u}}_{kl}\right]}_{\times }\left({\left({\mathbf{R}}_k{\mathbf{R}}_{\text{cam}}^{\text{IMU}}\right)}^T\left({\mathbf{p}}_l-\left({\mathbf{t}}_k+{\mathbf{R}}_k{\mathbf{t}}_{\text{cam}}^{\text{IMU}}\right)\right)\right)=0_3.\text{\hspace{1em}(18)}$$
当存在测量噪声时，式(18) 变为：
$${\left[{\mathbf{u}}_{kl}\right]}_{\times }\left({\left({\mathbf{R}}_k{\mathbf{R}}_{\text{cam}}^{\text{IMU}}\right)}^T\left({\mathbf{p}}_l-\left({\mathbf{t}}_k+{\mathbf{R}}_k{\mathbf{t}}_{\text{cam}}^{\text{IMU}}\right)\right)\right)={\mathbf{\eta }}_{kl}^{\text{cam}}.\text{\hspace{1em}(19)}$$
其中 $H$ 是时间范围 $H$ 内的连续帧集，${\Omega }_{kj}^{\text{IMU}}\in {\mathbb{R}}^{9\times 9}$ 是在(14)中引入的噪声向量 ${\eta }_{kj}^{\text{IMU}}$ 的信息矩阵。

虽然 IMU 测量值限制了未来时间范围 $H$ 中的状态，但时间 $k+H$ 处的预测信息矩阵也会受到时间 $k$ 处初始信息矩阵的影响。该信息矩阵称为 ${\bar{\Omega }}_k^{\text{PRIOR}}\in {\mathbb{R}}^{9\times 9}$，由 VIN 估计器计算和维护，可以理解为时间 $k$ 处状态的先验信息。该信息矩阵的存在导致在表达式 ${\bar{\Omega }}_{k:k+H}$ 中出现一个附加项，${\bar{\Omega }}_{k:k+H}$ 是在选择任何视觉测量之前的状态估计信息矩阵，如式(5)所示。具体而言，${\bar{\Omega }}_{k:k+H}$ 可以写成：

$${\bar{\Omega }}_{k:k+H}={\bar{\Omega }}_{k:k+H}^{\text{IMU}}+{\bar{\Omega }}_{k:k+H}^{\text{PRIOR}}\text{\hspace{1em}(16)}$$

其中 ${\bar{\Omega }}_{k:k+H}^{\text{PRIOR}}$ 是一个矩阵，除了左上角的 $9\times 9$ 块外，其余部分均为零，该块等于 ${\bar{\Omega }}_k^{\text{PRIOR}}$，以反映我们对时间 $k$ 状态的先验信息。矩阵 ${\bar{\Omega }}_{k:k+H}$ 表示在选择视觉测量之前状态 ${\mathbf{x}}_{k:k+H}$ 的最佳估计的信息矩阵，并遵循我们在(5)中已介绍的符号。先验和测量的存在与标准固定滞后平滑器完全类似，而这里我们利用了线性模型的优势。

2. \textbf{视觉模型}: 同样，对于视觉测量，我们旨在设计一个线性测量模型，以简化实际的（非线性）透视投影模型。为此，我们必须将像素测量表示为我们所需估计的未知状态的线性函数。

其中 ${\mathbf{\eta }}_{kl}^{\text{cam}}$ 是具有已知协方差的零均值随机噪声。假设通过陀螺仪积分已知旋转，则模型 (19) 中的未知量为机器人位置 ${\mathbf{t}}_k$（它是我们的状态向量 ${\mathbf{x}}_{k:k+H}$ 的一部分）和观测到的 $3$-D 地标位置 ${\mathbf{p}}_l$。重新排列这些项，我们得到：
$${\left[{\mathbf{u}}_{kl}\right]}_{\times }\left({\left({\mathbf{R}}_{\text{cam}}^{\text{IMU}}\right)}^T{\mathbf{t}}_{\text{cam}}^{\text{IMU}}\right)={\left[{\mathbf{u}}_{kl}\right]}_{\times }\left({\left({\mathbf{R}}_k{\mathbf{R}}_{\text{cam}}^{\text{IMU}}\right)}^T\left({\mathbf{t}}_k-{\mathbf{p}}_l\right)\right)+{\mathbf{\eta }}_{kl}^{\text{cam}}.$$
在上述方程中，唯一的未知量为 ${\mathbf{t}}_k$ 和 ${\mathbf{p}}_l$，因此该模型在状态下是线性的，可以写成矩阵形式：
$${\mathbf{z}}_{kl}^{\text{cam}}={\mathbf{F}}_{kl}{\mathbf{x}}_{k:k+H}+{\mathbf{E}}_{kl}{\mathbf{p}}_l+{\mathbf{\eta }}_{kl}^{\text{cam}},\text{\hspace{1em}(20)}$$
其中 ${\mathbf{z}}_{kl}^{\text{cam}}={\left[{\mathbf{u}}_{kl}\right]}_{\times }{\left({\mathbf{R}}_{\text{cam}}^{\text{IMU}}\right)}^T{\mathbf{t}}_{\text{cam}}^{\text{IMU}}$，且 ${\mathbf{F}}_{kl}\in {\mathbb{R}}^{3\times 9(H+1)}$ 和 ${\mathbf{E}}_{kl}\in {\mathbb{R}}^{3\times 3}$ 是合适的矩阵。为了进行三角测量，必须在多帧中观测到一个点。将线性系统 (20) 在每个可观测地标 $l$ 的姿态下堆叠起来，我们得到一个单一的线性系统。
$${\mathbf{z}}_l^{\text{cam}}={\mathbf{F}}_l{\mathbf{x}}_{k:k+H}+{\mathbf{E}}_l{\mathbf{p}}_l+{\mathbf{\eta }}_l^{\text{cam}}\text{\hspace{1em}(21)}$$
其中，${\mathbf{z}}_l^{\text{cam}}\in {\mathbb{R}}^{3n}$，${\mathbf{F}}_l\in {\mathbb{R}}^{3n\times 9(H+1)}$ 和 ${\mathbf{E}}_l\in {\mathbb{R}}^{3n\times 3}$ 分别通过将 ${\mathbf{z}}_{kl}^{\text{cam}}$、${\mathbf{F}}_{kl}$ 和 ${\mathbf{E}}_{kl}$ 按行堆叠获得，这里 $n$ 表示地标 $l$ 可见的帧数。对于 IMU 模型，${\mathbf{z}}_l^{\text{cam}}$ 的表达式对我们的推导并无实际影响，因为它不影响未来状态的协方差。另一方面，${\mathbf{F}}_l$ 和 ${\mathbf{E}}_l$ 依赖于未来测量值 ${\mathbf{u}}_{kl}$：因此，计算这些矩阵需要对水平线上每一帧中的 ${\mathbf{p}}_l$ 进行像素投影模拟。当使用立体相机时，我们可以估计 ${\mathbf{p}}_l$，因此可以轻松地将其投影到未来的帧中。在单目设置下，我们可以从 VIN 后端的现有特征中猜测新特征的深度。我们需要指出，在模拟未来时间范围内的像素投影时，还需要执行可见性检查（即，我们使用相机投影模型判断地标是否投影到图像帧内），从而根据车辆的预期运动和相机的视场范围限制未来视觉测量的集合。这一方面对于使我们的特征选择方法具有“预测性”特征至关重要。正如下面命题 1 所讨论的那样，生成的模型无缝地捕捉了一个直观事实：那些在多个帧中保持可见且可以被跟踪的地标具有更高的信息量。

现在我们注意到，不能直接使用线性模型 (21) 来估计状态向量 ${\mathbf{x}}_{k:k+H}$，因为它包含地标 $l$ 的未知位置。一种解决这一问题的方法是将 $3$-D 点包含在状态向量中。然而，这种做法有两个缺点：首先，将地标作为状态的一部分会显著增加状态空间的维度（从而增加式 (9) 中矩阵的维度）。其次，这可能会导致性能指标出现不良行为，例如，这些指标可能会倾向于选择最小化远处 $3$-D 点不确定性的特征，而忽略了我们真正感兴趣的变量（即机器人的状态）。为避免这种不良影响，我们通过 Schur 补技巧 \cite{72} 从估计中解析地消除 $3$-D 点。首先，根据线性测量 (21)，我们写出联合状态 $[{\mathbf{x}}_{k:k+H}{\mathbf{p}}_l]$ 的信息矩阵：
$${\mathbf{\Omega }}_{k:k+H}^{(l)}=\left[\begin{array}{cc}{\mathbf{F}}_l^{\top }{\mathbf{F}}_l& {\mathbf{F}}_l^{\top }{\mathbf{E}}_l\\ {\mathbf{E}}_l^{\top }{\mathbf{F}}_l& {\mathbf{E}}_l^{\top }{\mathbf{E}}_l\end{array}\right]\in {\mathbb{R}}^{(9H+12)\times (9H+12)}.\text{\hspace{1em}(22)}$$
为简单起见，我们假设像素测量噪声为单位矩阵。利用 Schur 补技巧，我们将地标 $l$ 边缘化，并得到状态 ${\mathbf{x}}_{k:k+H}$ 在测量 (21) 下的信息矩阵：

$${\Delta }_l={\mathbf{F}}_l^{\top }{\mathbf{F}}_l-{\mathbf{F}}_l^{\top }{\mathbf{E}}_l{\left({\mathbf{E}}_l^{\top }{\mathbf{E}}_l\right)}^{-1}{\mathbf{E}}_l^{\top }{\mathbf{F}}_l\in {\mathbb{R}}^{9(H+1)\times 9(H+1)}.\text{\hspace{1em}(23)}$$

等式 (23) 表示单个地标 $l$ 的测量对状态估计信息矩阵的（附加）贡献。这是我们在式 (5) 中已称为 ${\Delta }_l$ 的矩阵。

矩阵 ${\Delta }_l$ 是稀疏的，其稀疏模式由地标 $l$ 在不同帧之间的共视性决定 \cite{73}。值得注意的是，${\left({\mathbf{E}}_l^{\top }{\mathbf{E}}_l\right)}^{-1}$ 是地标位置估计的协方差 \cite{73}，且只要地标 $l$ 可以被三角测量，它就是可逆的。在我们的实现中，我们仅考虑可以被三角测量的地标（即 ${\left({\mathbf{E}}_l^{\top }{\mathbf{E}}_l\right)}^{-1}$ 可逆的地标）作为特征选择的候选对象。

以下命题正式证明，较长的特征轨迹会导致“更大”的 ${\Delta }_l$，因此公式 (9) 将鼓励选择具有长轨迹的特征。

\textbf{命题 1 (长特征轨迹)}: 考虑两个地标 $l_1$ 和 $l_2$，它们在时间 $k_1$ 之前具有相同的预测未来像素测量值（即 ${\mathbf{z}}_{\tau ,l_1}^{\text{cam}}={\mathbf{z}}_{\tau ,l_2}^{\text{cam}}$，其中 $\tau =k,\dots ,k_1$），但 $l_1$ 仅被跟踪到 $k_1$，而 $l_2$ 被跟踪到更晚的帧 $k_2$（其中 $k<k_1<k_2\le H$）。那么，有：

$${\Delta }_{l_1}⪯{\Delta }_{l_2}.$$

命题 1 的证明见附录 B。

命题 1 确保具有长轨迹的特征携带“更大”的信息内容 ${\Delta }_l$，因此对目标 (9) 的贡献更大（回顾一下，对于两种性能指标选择，均有：

$$f\left({\bar{\mathbf{\Omega }}}_{k:k+H}+{\Delta }_{l_1}\right)\le f\left({\bar{\mathbf{\Omega }}}_{k:k+H}+{\Delta }_{l_2}\right)\text{当且仅当}{\Delta }_{l_1}⪯{\Delta }_{l_2}.$$

但需要注意的是，命题 1 确保长特征轨迹仅在未来测量值相同时“主导”短特征轨迹。因此，通常情况下，基于特征轨迹长度的启发式选择可能会为问题 (1) 提供次优解（直观地说，这种选择不会考虑特征的几何形状）。

\textit{备注 2 (线性测量模型)}: 第 III-E1 和 III-E2 节提供了惯性和视觉测量的线性测量模型。具体来说，我们假设在较短的时间范围内旋转是已知的，这使我们能够获得关于未知机器人状态的线性测量模型。在我们的框架中，可以直接使用 VIN \cite{68} 中常用的非线性惯性模型和透视模型的线性化版本。

我们选择设计线性模型的原因有三点。首先，我们操作于一个较小的状态空间（不包括旋转和陀螺仪偏置），从而加快矩阵运算速度。其次，我们避免了对非线性模型进行线性化的实际计算开销。第三，由于模型的简洁性，我们能够更好地理解特征选择机制的几何本质（参见 IV-D 节）。

注意力分配：算法和性能保证

在本节中，我们讨论求解特征选择问题 (1) 的计算方法，以找到一组近似解特征。众所周知，精确求解问题 (1) 的最优子集 ${\mathbf{S}}^{\ast }$ 是 NP-hard 的 \cite{46}，因此在实际问题中，我们无法期望找到高效的算法来计算 ${\mathbf{S}}^{\ast }$。我们在本文中采用的解决方案是设计近似算法，这些算法在计算上是高效的，并提供性能保证（简而言之，所生成的集合与最优子集 ${\mathbf{S}}^{\ast }$ 相差无几）。我们需要指出的是，我们设计的是一种隐蔽注意力机制：我们的算法仅选择一组必须保留并用于状态估计的特征，而我们不会尝试主动控制相机的运动。

接下来，我们介绍两类算法。第一类算法基于原始组合问题 (1) 的凸松弛。我们并不声称凸松弛是我们的原创贡献，因为它们在其他背景下已被多次提出，例如参见 \cite{43}。我们将在第 IV-A 和 IV-B 节中简要回顾凸松弛及其相应的性能保证。第二类近似算法包括贪婪选择方法，并将在第 IV-C 节中进行更详细的讨论。我们将在第 IV-D 节中为贪婪算法提供性能保证。

A.凸松弛

本节介绍一种凸松弛方法来计算问题 (1) 的近似解。

使用式 (9)，我们将问题 (1) 重新明确地写为：

$$\underset{S\subseteq F}{\max }f\left({\bar{\mathbf{\Omega }}}_{k:k+H}+\sum _{l\in S}{p}_l{\Delta }_l\right)\text{subject to}|S|\le \kappa ,\text{\hspace{1em}(24)}$$

其中 $f(\cdot )$ 表示 $f_{\lambda }(\cdot )$ 或 $f_{\det }(\cdot )$（目前不需要区分这两个度量）。

引入二元变量 $s_l$，对于 $l=1,\dots ,N$，我们将式 (24) 等效地重写为：

max ⁡ s 1 , … , s N f ( Ω ˉ k : k + H + ∑ l = 1 N s l p l Δ l ) subject to  ∑ l = 1 N s l ≤ κ ,   s l ∈ { 0 , 1 } ,   ∀ l ∈ { 1 , … , N } . (25) \max_{s_1, \dots, s_N} f\left( \bar{\boldsymbol{\Omega}}_{k:k+H} + \sum_{l=1}^N s_l p_l \Delta_l \right) \quad \text{subject to} \ \sum_{l=1}^N s_l \leq \kappa, \ s_l \in \{0, 1\}, \ \forall l \in \{1, \dots, N\}. \tag{25} s1​,…,sN​max​f(Ωˉk:k+H​+l=1∑N​sl​pl​Δl​)subject to l=1∑N​sl​≤κ, sl​∈{0,1}, ∀l∈{1,…,N}.(25)

问题 (25) 是一个二元优化问题。虽然该问题能够返回最优子集 ${\mathbf{S}}^{\ast }$，但由于约束 $s_l$ 必须是二元的，它依然是 NP-hard 的。

问题 (25) 可以进行简单的凸松弛，表示为：

f cvx = max ⁡ s 1 , … , s N f ( Ω ˉ k : k + H + ∑ l = 1 N s l p l Δ l ) subject to  ∑ l = 1 N s l ≤ κ ,   s l ∈ [ 0 , 1 ] ,   ∀ l ∈ { 1 , … , N } . (26) f_{\text{cvx}} = \max_{s_1, \dots, s_N} f\left( \bar{\boldsymbol{\Omega}}_{k:k+H} + \sum_{l=1}^N s_l p_l \Delta_l \right) \quad \text{subject to} \ \sum_{l=1}^N s_l \leq \kappa, \ s_l \in [0, 1], \ \forall l \in \{1, \dots, N\}. \tag{26} fcvx​=s1​,…,sN​max​f(Ωˉk:k+H​+l=1∑N​sl​pl​Δl​)subject to l=1∑N​sl​≤κ, sl​∈[0,1], ∀l∈{1,…,N}.(26)

其中，二元约束 s l ∈ { 0 , 1 } s_l \in \{0, 1\} sl​∈{0,1} 被凸约束 $s_l\in [0,1]$ 取代。问题 (26) 的凸性源于我们在线性不等式约束下最大化一个凹成本函数的事实 \cite{7}。

这种凸松弛在其他背景下已被多次提出（例如，参见 \cite{43}）。(26) 的解 $s_1^{\ast },\dots ,s_N^{\ast }$ 通常不是二元的，因此需要一个舍入程序来区分需要丢弃的特征（$s_l=0$）和需要选择的特征（$s_l=1$）。一种常见的舍入方法是简单地选择具有最大 $s_l$ 的 $\kappa$ 个特征，而随机化舍入方法也已被提出 \cite{38}。

我们采用前者，并将 ${\mathbf{S}}^{\circ }$ 定义为包含 $\kappa$ 个具有最大 $s_l^{\ast }$ 的特征索引的集合，其中 $s_1^{\ast },\dots ,s_N^{\ast }$ 是问题 (26) 的最优解。

B.凸松弛的性能保证

尽管没有明确的（\textit{先验}）性能保证来衡量集合 ${\mathbf{S}}^{\circ }$ 的质量，但凸松弛 (26) 在实践中已被观察到具有良好的效果。

我们令 $f_{\text{cvx}}$ 表示松弛问题 (26) 的最优目标值，$f({\mathbf{S}}^{\circ })$ 表示由舍入解所达到的目标值，$f({\mathbf{S}}^{\ast })$ 表示原始 NP-hard 问题 (25) 的最优解。由此，我们可以通过以下关系轻松获得一个\textit{后验}性能界限：

$$f({\mathbf{S}}^{\circ })\le f({\mathbf{S}}^{\ast })\le f_{\text{cvx}}.\text{\hspace{1em}(27)}$$

其中，第一个不等式源于 ${\mathbf{S}}^{\ast }$ 的最优性（任意 $\kappa$ 个特征的子集的代价最多为 $f({\mathbf{S}}^{\ast })$），而第二个不等式来自于 (26) 是原始问题的松弛形式。

不等式链 (27) 提供了一个简单的（\textit{后验}）性能界限，衡量了由凸松弛 (26) 所产生的集合质量：

$$f({\mathbf{S}}^{\ast })-f({\mathbf{S}}^{\circ })\le f_{\text{cvx}}-f({\mathbf{S}}^{\circ }),\text{\hspace{1em}(28)}$$

即，子集 ${\mathbf{S}}^{\circ }$ 的次优性间隔 $f({\mathbf{S}}^{\ast })-f({\mathbf{S}}^{\circ })$ 由差异 $f_{\text{cvx}}-f({\mathbf{S}}^{\circ })$ 界定，而该差异可在求解 (26) 之后\textit{后验}计算得出。

C.贪婪算法与惰性求值

本节介绍第二种近似解决问题 (1) 的方法。与第 IV-A 节的凸松弛方法不同，我们在这里考虑一种贪婪算法，该算法选择 $\kappa$ 个特征，以（近似）最大化成本 $f(\cdot )$。

该算法从空集 $\mathbf{S}^# $ 开始，并执行 $\kappa$ 次迭代。在每次迭代中，它都会添加一个特征，如果将其添加到 ${\mathbf{S}}^{\#}$ 中，会导致成本函数的最大增量。算法的伪代码在\textbf{算法 1} 中给出。

在第 3 行，算法从空集开始。第 4 行中的 for'' 循环执行 $\kappa$ 次：每次迭代时，当前最优特征会被添加到子集 $\mathbf{S}^\#$（第 19 行）。第 11 行的 for’’ 循环的作用是计算使成本增量最大的特征（第 15–17 行）。其余部分实现了一种惰性求值机制。对于每个特征 $l$，我们计算成本 f ( S # ∪ { l } ) f(\mathbf{S}^\# \cup \{l\}) f(S#∪{l}) 的上界（第 6 行）。接着，特征根据该上界进行降序排序（第 8 行）。这种机制的优势在于，通过将当前最佳特征与该上界进行比较（第 12 行），我们可以跳过那些保证只能获得较小成本的特征。

显然，如果计算上界的速度快于计算实际成本，那么惰性求值是有利的。以下命题为我们的成本函数提供了两个有用且计算开销较低的上界。

\textbf{命题 3 (log det 的上界)}: Hadamard 不等式 \cite{75}，Th. 7.8.1。对于正定矩阵 $\mathbf{M}\in {\mathbb{R}}^{n\times n}$，其对角线元素为 $M_{ii}$，成立：
$$\det (\mathbf{M})\le \prod _{i=1}^n{M}_{ii}\iff \log \det (\mathbf{M})\le \sum _{i=1}^n\log M_{ii}.\text{\hspace{1em}(29)}$$
\textbf{命题 4 (特征值扰动界 \cite{76})}: 给定埃尔米特矩阵 $\mathbf{M},\Delta \in {\mathbb{R}}^{n\times n}$，并设 ${\lambda }_i(\mathbf{M})$ 表示 $\mathbf{M}$ 的第 $i$ 个特征值，下列不等式成立：
$$|{\lambda }_i(\mathbf{M}+\Delta )-{\lambda }_i(\mathbf{M})|\le \Vert \Delta \Vert ,\text{\hspace{1em}(30)}$$
$$\underset{j}{\min }|{\lambda }_i(\mathbf{M})-{\lambda }_j(\mathbf{M}+\Delta )|\le \Vert \Delta {\mathbf{v}}_i\Vert ,\text{\hspace{1em}(31)}$$
其中 ${\mathbf{v}}_i$ 是与 ${\lambda }_i(\mathbf{M})$ 关联的 $\mathbf{M}$ 的特征向量。

等式 (30) 是经典 Weyl 不等式的重述，而等式 (31) 是 Ipsen 和 Nadler \cite{76} 提出的更严格的界限。为了阐明命题 4 中的界限如何为我们提供 ${\lambda }_{\min }$ 的上界，我们证明以下结果：

\textbf{推论 5 ( ${\lambda }_{\min }$ 的上界)}: 给定两个对称且半正定矩阵 $\mathbf{M},\Delta \in {\mathbb{R}}^{n\times n}$，以下不等式成立：

$${\lambda }_{\min }(\mathbf{M}+\Delta )\le {\lambda }_{\min }(\mathbf{M})+\Vert \Delta {\mathbf{v}}_{\min }\Vert ,\text{\hspace{1em}(32)}$$

其中，${\mathbf{v}}_{\min }$ 是与 $\mathbf{M}$ 的最小特征值 ${\lambda }_{\min }(\mathbf{M})$ 相关联的特征向量。

推论 5 的证明见附录 C。虽然算法 1 突出了贪婪算法的简单性，但目前尚不清楚该算法是否能产生良好的特征子集。我们将在下一节中讨论这一问题。

贪婪算法的性能保证

本节表明，贪婪算法（\textbf{算法 1}）具备可证明的次优性界限。这些界限保证了贪婪选择的表现不会比最优策略差太多。本节分别处理第 III-A 节中提出的两个指标，因为它们对应的性能保证本质上是不同的。

我们的结果基于最近关于\textbf{子模性}和\textbf{子模最大化}的文献。在深入讨论每个指标的性能保证之前，我们先提供一些初步定义，专家读者可以安全地跳过这些内容。

\textbf{定义 6 (规范化和单调集合函数 \cite{77})}: 一个集合函数 $f:2^F\to \mathbb{R}$ 被称为\textbf{规范化}的，如果 $f(\varnothing )=0$；如果对于任意子集 $A\subseteq B\subseteq F$，有 $f(A)\le f(B)$，则称 $f$ 是\textbf{单调}的（非递减的）。

\textbf{定义 7 (子模性 \cite{77})}: 一个集合函数 $f:2^F\to \mathbb{R}$ 是\textbf{子模}的，如果对于任意子集 $A\subseteq B\subseteq F$，以及任意元素 $e\in F\setminus B$，成立：

f ( A ∪ { e } ) − f ( A ) ≥ f ( B ∪ { e } ) − f ( B ) . (33) f(A \cup \{e\}) - f(A) \geq f(B \cup \{e\}) - f(B). \tag{33} f(A∪{e})−f(A)≥f(B∪{e})−f(B).(33)

子模性形式化了\textbf{收益递减}的概念：将测量结果添加到一小组测量集合中比添加到一大组测量集合中更为有利。我们对子模性的兴趣源于以下结果。

\textbf{命题 8 (接近最优的子模最大化 \cite{77})}: 给定一个规范化、单调且子模的集合函数 $f:2^F\to \mathbb{R}$，设 ${\mathbf{S}}^{\ast }$ 是最大化问题 (1) 的最优解，则由贪婪算法 1 计算得到的集合 ${\mathbf{S}}^{\#}$ 满足：

$$f({\mathbf{S}}^{\#})\ge \left(1-\frac{1}{e}\right)f({\mathbf{S}}^{\ast })\approx 0.63f({\mathbf{S}}^{\ast }).\text{\hspace{1em}(34)}$$

该界限保证了贪婪算法的最坏情况性能不会偏离最优解太多。接下来，我们将这一结果应用于我们的特征选择问题。

\subsubsection{log det 的次优性保证} 可以证明，\textbf{log det} 是相对于用于估计的测量集合的子模函数。该结果及其对应的性能保证形式化如下：

\textbf{命题 9 (log det 的子模性 \cite{47})}: 式 (4) 中定义的集合函数 $f_{\det }(\mathbf{S})$ 是单调且子模的。此外，以 $f_{\det }(\mathbf{S})$ 作为目标函数应用于问题 (1) 的贪婪算法享有以下性能保证：

$$f_{\det }(\mathbf{S})\ge \left(1-\frac{1}{e}\right)f_{\det }({\mathbf{S}}^{\ast })+\frac{f_{\det }(\varnothing )}{e}.\text{\hspace{1em}(35)}$$

该结果在文献 \cite{47} 中得到了证明，随后在 \cite{50} 中进一步修正，以解释对规范化函数的需求。式 (35) 中的额外项 $\frac{f_{\det }(\varnothing )}{e}$ 确实是通过将命题 8 应用于规范化函数 $f_{\det }(\mathbf{S})-f_{\det }(\varnothing )$ 而得出的。

\subsubsection{${\lambda }_{\min }$ 的次优性保证}
目前，尚无结果可以直接约束贪婪算法在最大化信息矩阵的最小特征值（或等价地，最小化协方差的最大特征值）时的次优性差距。事实上，相关工作提供了反例，表明该度量通常不是子模的，而贪婪算法在实践中表现良好 \cite{50}。在本节中，我们提供了第一个结果，表明尽管 $f_{\lambda }(\mathbf{S})$ 并非严格的子模函数，但它与子模函数相差不远。接下来我们将更精确地描述这一概念。

\textbf{定义 10 (子模比 \cite{69, 78})}: 非负集合函数 $f(\cdot )$ 关于集合 $\mathbf{S}$ 和整数 $\kappa \ge 1$ 的\textbf{子模比}定义为：

γ S : = min ⁡ L ⊆ S ,   E : ∣ E ∣ ≤ κ ,   E ∩ L = ∅ ∑ e ∈ E ( f ( L ∪ { e } ) − f ( L ) ) f ( L ∪ E ) − f ( L ) . (36) \gamma_\mathbf{S} := \min_{L \subseteq \mathbf{S}, \, E: |E| \leq \kappa, \, E \cap L = \emptyset} \frac{\sum_{e \in E} \left( f(L \cup \{e\}) - f(L) \right)}{f(L \cup E) - f(L)}. \tag{36} γS​:=L⊆S,E:∣E∣≤κ,E∩L=∅min​f(L∪E)−f(L)∑e∈E​(f(L∪{e})−f(L))​.(36)

可以证明，如果 ${\gamma }_{\mathbf{S}}\ge 1$，则函数 $f(\cdot )$ 是子模函数。然而，在当前背景下，我们感兴趣的是\textbf{子模比}，因为它允许实现一个更宽松的次优性界限，这将在下文的命题中进行描述。

\textbf{命题 11 (近似子模最大化 \cite{69, 78})}: 设 $f(\cdot )$ 是非负单调集合函数，${\mathbf{S}}^{\ast }$ 是最大化问题 (1) 的最优解。则由贪婪算法 1 计算得到的集合 ${\mathbf{S}}^{\#}$ 满足：

$$f({\mathbf{S}}^{\#})\ge \left(1-e^{-{\gamma }_{{\mathbf{S}}^{\#}}}\right)f({\mathbf{S}}^{\ast }),\text{\hspace{1em}(37)}$$

其中，${\gamma }_{{\mathbf{S}}^{\#}}$ 是函数 $f(\cdot )$ 关于集合 ${\mathbf{S}}^{\#}$ 的子模比，且 $\kappa =|{\mathbf{S}}^{\#}|$。

命题 11 表明，只要 ${\gamma }_{{\mathbf{S}}^{\#}}>0$，就可以提供一个乘法次优性界限。接下来，我们将证明在最大化最小特征值时，通常满足这一条件。

\textbf{命题 12 (${\lambda }_{\min }$ 的非零子模比)}: 称 ${\mathbf{S}}^{\#}$ 为贪婪算法最大化 ${\lambda }_{\min }$ 所返回的集合。对于任意集合 $L\subseteq {\mathbf{S}}^{\#}$，我们称 $\bar{\mu }$ 为矩阵 ${\bar{\Omega }}_{k:k+H}+{\sum }_{l\in L}{\Delta }_l$ 的最小特征值对应的特征向量。此外，我们将 ${\bar{\mu }}_0,{\bar{\mu }}_2,\dots ,{\bar{\mu }}_H\in {\mathbb{R}}^3$ 称为 $\bar{\mu }$ 的子向量，分别对应于机器人在时间 $k,\dots ,k+H$ 的位置。如果存在 $i,j$ 使得 ${\bar{\mu }}_i\ne {\bar{\mu }}_j$，则 ${\lambda }_{\min }$ 的子模比有界于零。

命题 12 的证明见附录 D。直观上，命题 12 指出，只要最大不确定性的方向沿着未来时间范围发生变化，子模比就不会消失。以下推论是命题 12 的直接结果：

\textbf{推论 13 (${\lambda }_{\min }$ 的近似子模性)}: 式 (9) 中定义的集合函数 $f_{\lambda }(\mathbf{S})$ 是单调的。此外，在命题 12 的假设下，以 $f_{\lambda }(\mathbf{S})$ 为目标函数应用于问题 (1) 的贪婪算法，在非零 ${\gamma }_{{\mathbf{S}}^{\#}}$ 的情况下，享有命题 11 中所给出的性能保证。

\textbf{证明}: 单调性由 Weyl 不等式 \cite{76} 推导得出。贪婪算法的性能保证由命题 11 和命题 12 推导得出。

\textbf{推论 13} 保证了命题 11 的近似界限不会消失，因此贪婪算法总是能够在常数因子范围内逼近最优解。第 V 节中的经验证据进一步证实，应用于最大化 $f_{\lambda }(\mathbf{S})$ 的贪婪算法表现出色，在所有测试实例中均能产生接近最优的结果。

\textbf{注释 14 (关于 ${\lambda }_{\min }$ 贪婪选择的几何直觉)}: 我们的线性模型提供了对贪婪选择背后几何特性的更深入理解。贪婪选择会奖励那些具有较大目标 ${\lambda }_{\min }({\bar{\Omega }}_{k:k+H}+{\Delta }_l)$ 的特征，或者等效地，具有较大边际增益 ${\lambda }_{\min }({\bar{\Omega }}_{k:k+H}+{\Delta }_l)-{\lambda }_{\min }({\bar{\Omega }}_{k:k+H})$ 的特征。以下关系链提供了对哪些特征能够产生较大边际增益的几何理解：
$${\lambda }_{\min }({\bar{\Omega }}_{k:k+H}+{\Delta }_l)-{\lambda }_{\min }({\bar{\Omega }}_{k:k+H})$$
(来自瑞利商)

$$=\underset{\Vert \nu \Vert =1}{\min }{\nu }^T({\bar{\Omega }}_{k:k+H}+{\Delta }_l)\nu -\underset{\Vert \mu \Vert =1}{\min }{\mu }^T({\bar{\Omega }}_{k:k+H})\mu$$
(设 $\bar{\mu }$ 为第二个求和项的最小化解)

$$=\underset{\Vert \nu \Vert =1}{\min }{\nu }^T({\bar{\Omega }}_{k:k+H}+{\Delta }_l)\nu -{\bar{\mu }}^T({\bar{\Omega }}_{k:k+H})\bar{\mu }$$

(用第一个求和项的次优解 $\bar{\mu }$ 代替)

$$\le {\bar{\mu }}^T({\bar{\Omega }}_{k:k+H}+{\Delta }_l)\bar{\mu }-{\bar{\mu }}^T({\bar{\Omega }}_{k:k+H})\bar{\mu }$$

(简化并替换 ${\Delta }_l$ 的表达式)

$$={\bar{\mu }}^T{\Delta }_l\bar{\mu }={\bar{\mu }}^T{F}_l^T\left(I-E_l(E_l^T{E}_l{)}^{-1}{E}_l^T\right)F_l\bar{\mu }$$

(定义幂等矩阵 $Q=(I-E_l(E_l^T{E}_l{)}^{-1}{E}_l^T)$)

$$={\bar{\mu }}^T{F}_l^{T}QQ{F}_l\bar{\mu }=\Vert \mathbf{Q}{F}_l\bar{\mu }{\Vert }^2$$

(利用三角不等式并代入 $F_l$)

$$\le \Vert \mathbf{Q}{\Vert }^2\Vert F_l\bar{\mu }{\Vert }^2=\Vert F_l\bar{\mu }{\Vert }^2=\sum _{k=0}^H\Vert [u_{kl}{]}_{\times }(R_{\text{cam},k}^W{)}^T{\bar{\mu }}_k{\Vert }^2.$$

其中 ${\bar{\mu }}_k$ 是 $\bar{\mu }$ 中与时间 $k$ 处机器人位置坐标相对应的子向量。直观地说，不等式表明当 $\Vert [u_{kl}{]}_{\times }(R_{\text{cam},k}^W{)}^T{\bar{\mu }}_k\Vert$ 较小时，边际增益也较小。这意味着当我们选择地标观测时，测量方位 $u_{kl}$ 几乎平行于较大不确定性方向 ${\bar{\mu }}_k$（在相机坐标系中通过旋转 $(R_{\text{cam},k}^W{)}^T$ 进行变换）时，边际增益较小。例如，如果前向方向的不确定性较大，那么选择机器人正前方的特征（即方位与最大不确定性方向平行）是不方便的。因此，贪婪方法会选择图像外围的特征，这在直观上提供了一种更有效的方法来减少不确定性。

实验

本节提供了三组实验结果。第一组测试（第 V-A 节）表明，贪婪算法在解决问题 (1) 时获得了接近最优的解，同时比第 IV-A 节中讨论的凸松弛的通用求解器运行更快。第二组测试（第 V-E 节）在实际模拟中评估了我们的 c++ 流水线，结果表明我们的特征选择技术提升了 VIN 的性能；同一节还展示了我们惰性评估方法的优势。第三组测试（第 V-F 节）评估了我们的方法在敏捷微型飞行器（MAV）收集的真实数据上的表现。\

A.贪婪算法在特征选择中的评估

本节回答了以下问题：\textit{贪婪算法 1 在（近似）解决组合优化问题 (1) 方面表现如何？} 具体来说，我们表明，对于成本函数 (9) 的两种选择，贪婪算法都能够找到问题 (1) 的近似最优解；我们还表明，第 IV-A 节中提出的凸松弛方法也能够找到近似最优解，但计算开销更大。

B.测试设置

为了生成问题 (1) 的随机实例，我们考虑一个以 $2\text{m/s}$ 的恒定速度沿直线移动的机器人。机器人配备了一个采样周期为 $\delta =0.01\text{s}$ 的 IMU；我们选择加速度计噪声密度为 $0.02\text{m}/({\text{s}}^2\sqrt{\text{Hz}})$，加速度计偏差连续时间噪声密度为 $0.03\text{m}/({\text{s}}^3\sqrt{\text{Hz}})$。我们还模拟了一个机载单目摄像头，该摄像头以 $0.5\text{s}$ 的（关键）帧速率测量随机分布在环境中的 3-D 点。机器人必须从 $N$ 个可用的视觉测量中选择一组 $\kappa$ 个特征。

我们假设在特征选择时，机器人的位置协方差为 $10^{-2}\cdot {\mathbf{I}}_3$，而其速度和加速度计偏差的协方差分别为 $10^{-2}\cdot {\mathbf{I}}_3$ 和 $10^{-4}\cdot {\mathbf{I}}_3$。利用这些信息，我们构建了矩阵 ${\bar{\Omega }}_{k:k+H}$，其中预测范围为 $2.5\text{s}$。此外，根据可用的特征测量，我们构建了矩阵 ${\Delta }_l$；在这些测试中，我们假设 $p_l=1$，即在特征选择过程中忽略外观特性。

C. 技术方法与评估指标

我们对比了两种求解方法 (1)：算法1中提出的贪心算法和凸松弛方法 (26)。

我们使用\textbf{CVX/MOSEK}作为 (26) 的解析器/求解器来实现凸松弛方法，然后按照第IV-A节中描述的方法计算了四舍五入的解。在本节的评估中，我们在MATLAB中实现了贪心算法和凸松弛方法。我们分别针对目标函数$f_{\lambda }$ 和 $f_{\text{det}}$（定义在(9)中）的每个选择，评估这些方法的表现。理想情况下，我们需要比较各个技术获得的目标值与最优目标值。

然而，最优目标值很难计算，而暴力求解方法即使对于相对较小的问题实例也极为缓慢。幸运的是，凸松弛方法 (26) 还会提供问题 (1) 最优成本的上界 [参见(27)]，因此我们可以利用这个上界来衡量贪心算法和凸松弛方法中四舍五入解与最优解之间的差距。

D. 结果

我们考虑规模不断增大的问题，给定 $N$ 个特征，我们需要从中选择一半的特征（$\kappa =N/2$），以最大化目标函数 $(1)$。对于每个 $N$，我们计算 50 次蒙特卡洛统计数据。

图 1(a) 显示了不同技术在特征数量 $N$ 增加时所获得的最小特征值目标 $f_{\lambda }$。除了贪心算法（greedy）、凸松弛的舍入解（标签：\textit{rounded}）和放松目标（标签：\textit{relaxed}）外，我们还展示了通过随机选择 $\kappa$ 个特征子集（标签：\textit{random}）所获得的目标值。由于我们解决的是一个最大化问题，因此目标值越大越好。图 1(a) 表明，在所有测试实例中，\textit{greedy} 和 \textit{rounded} 解都与放松上界（\textit{relaxed}）一致（三条线几乎无法区分），因此它们都产生了最优解 [参见 (28)]。得到的解显然远胜于随机选择的结果。这一结果有些令人惊讶，因为最小特征值目标通常不具备子模性，而贪心算法的性能保证较弱（13）。然而，这一观察结果与其他领域的相关工作一致，例如 [49]。尽管贪心算法和凸松弛舍入解都能得到良好的结果，但解决凸问题 (26) 通常比计算贪心解更耗时：我们在 MATLAB 中实现的贪心算法（未使用惰性求值）的 CPU 时间大约为 0.4 秒（当 $N=50$ 时），而使用 CVX 求解大约需要 0.8 秒。

对于目标函数 $f_{\text{det}}$，也有类似的考虑。图 1(b) 显示了随着可用特征数量 $N$ 的增加，不同方法获得的对数行列式值。在这种情况下，算法同样需要选择 $\kappa =N/2$ 个特征。与之前的测试类似，\textit{greedy} 和 \textit{rounded} 方法在所有测试实例中都获得了最优解，达到了放松解的上界，并且表现明显优于随机选择。关于 CPU 时间，我们在 MATLAB 中实现的贪心算法优化 $f_{\text{det}}$ 的运行时间大约为 0.1 秒（当 $N=50$ 时），而 CVX 求解 (26) 大约需要 1 分钟以上。我们注意到，尽管 CVX 是一种最先进的通用凸优化求解器，但我们的分析并不排除设计专用快速求解器的可能性，例如 [79]；然而，这类尝试超出了本文的讨论范围。由于贪心算法的精度与凸松弛技术相当，但其速度明显快于通用凸优化求解器，因此在接下来的讨论中，我们将重点关注贪心算法。

E. 特征选择在 VIN 中的重要性

本节回答以下问题：\textit{通过解决 (1) 得出的特征选择是否会提高 VIN 的性能？} 接下来，我们展示了所提出的特征选择方法在实际的蒙特卡罗模拟中有效地提升了 VIN 的性能。

\subsubsection*{1) 测试设置}
我们采用了文献 [68] 中的基准测试问题，并在图 2(a) 中进行了展示作为测试设置。我们模拟了一个沿圆形轨迹移动的机器人，同时伴随正弦垂直运动。轨迹的总长度为 120 米。机载摄像头的焦距为 315 像素，运行频率为 2.5 Hz（模拟关键帧）。模拟的加速度和陀螺仪测量值与文献 [68] 中相同。

\subsubsection*{2) 实现与评估指标}
在本节中，我们专注于贪心算法，并使用这些算法来选择视觉特征的子集。我们使用 C++ 实现了贪心算法及函数 (9) 中所需矩阵的构建，采用 \textit{Eigen} 库进行对数行列式和最小特征值的计算。出于数值稳定性的考虑，我们并不直接计算行列式后取对数，而是通过矩阵的 \textit{Cholesky 分解} 直接计算对数行列式。此外，为了计算最小特征值，我们使用了 \textit{Eigen} 库的 SVD 函数。

我们的特征选择方法作为附加组件用于与文献 [68] 中描述的视觉惯性管道类似的框架中。我们的 VIN 管道通过结构无关的视觉模型和预集成的 IMU 模型来估计导航状态（机器人姿态、速度和 IMU 偏差），这些模型均在文献 [68] 中有所描述。整个实现基于 GTSAM 优化库 [80]。

我们的实现与文献 [68] 在三个重要方面存在不同：

\begin{enumerate}
\item 首先，在本文中，我们在固定滞后平滑方法中使用了 \textit{iSAM2} 算法；我们将平滑范围之外 6 秒的状态进行边缘化，这有助于控制延迟和内存需求。
\item 其次，我们没有采用 SVO 作为视觉前端。在本次模拟中，由于我们模拟了地标观测，因此不需要视觉前端。而在下一节中，我们将描述一个简单的真实世界视觉前端。
\item 最后，与直接将所有可用的测量数据输入 VIN 估计器不同，我们使用本文提出的特征选择算法，选择一小部分信息量丰富的视觉观测数据。
\end{enumerate}

\subsection*{方法评估}
在本节中，我们评估了我们方法的两个主要方面：

\begin{enumerate}
\item 我们证明，巧妙地选择特征确实能够显著影响 VIN 的准确性。
\item 我们展示了第 IV-C 节中讨论的惰性评估方法能够加速贪心算法的计算。
\end{enumerate}

我们使用两个指标来评估准确性：
\begin{itemize}
\item \textit{绝对平移误差}：即估计位置与实际位置之间的欧几里得距离。
\item \textit{相对平移误差}：通过计算时间 $k$ 和 $k+1$ 之间的估计平移与实际平移之间差异的欧几里得范数来衡量。相对平移误差能够量化估计值偏离真实值的速度。
\end{itemize}

由于在视觉惯性里程计中绝对位置不可直接观测，因此相对误差是一种更可靠的性能指标。在需要时，我们还报告了绝对和相对旋转误差（与平移误差的定义类似）。

\subsubsection*{3) 结果}
我们进行了 50 次蒙特卡罗模拟；在每次模拟中，我们向加速度计、陀螺仪和相机测量值添加了随机噪声。为了使模拟更加真实，测量噪声的统计特性与第 V-F 节中实际测试所使用的统计数据完全相同。

在每次模拟中，机器人执行 VIN 任务，并在每个相机帧中，从视野内的所有特征中选择 $\kappa =20$ 个视觉特征。我们比较了三种特征选择策略：

\begin{itemize}
\item 基于算法 1 的贪心选择，目标函数为特征值 $f_{\lambda }$（标签：\textit{minEig}）；
\item 基于对数行列式代价 $f_{\text{det}}$ 的贪心选择（标签：\textit{logDet}）；
\item 随机选择，从可用特征中随机抽取 $\kappa$ 个特征（标签：\textit{random}）。
\end{itemize}

\paragraph{图 2© 和 (d)} 显示了绝对平移误差和绝对旋转误差，这些误差取 50 次蒙特卡洛模拟的平均值。从图中可以明显看出，通过 \textit{logDet} 或 \textit{minEig} 进行特征选择，对 VIN 性能具有显著影响。

与随机选择相比，我们的方法在估计误差方面有了显著改善；然而，这两种方法的性能结果相似。从图中我们还注意到，绝对误差存在一些振荡现象：这是由于轨迹是圆形所导致的。总体上，这进一步强调了绝对误差指标可能并不是评估视觉惯性里程计性能的良好指标。

在这种情况下，相对误差指标证实了图 2© 和 (d) 的结果：平均平移误差和旋转误差的具体数值列于表 I 中；括号中报告了相对于随机选择的误差减少百分比。

\paragraph{图 2(b)} 报告了特征选择所需的 CPU 时间。

该图考虑了两种成本函数（\textit{logDet} 和 \textit{minEig}），并对比了使用我们提出的惰性求值方法（参见算法 1）与贪心算法的朴素实现之间的时间差异。朴素贪心算法（即禁用算法 1 第 12 行的停止条件）会始终测试每个特征的边际收益，导致 $\kappa N$ 次目标函数评估。使用惰性求值时，目标函数评估的次数取决于算法 1 中上界的紧密程度。

从图 2(b) 中可以看出，对于对数行列式成本（\textit{logDet}），惰性求值的优势并不明显；这并不奇怪，因为命题 3 中的 \textit{Hadamard} 不等式通常会提供一个较宽松的上界。另一方面，对于最小特征值目标（\textit{minEig}），惰性求值的优势非常显著，计算时间减少了约 20%。

算法 1（使用惰性求值）选择 $\kappa =20$ 个特征所需的平均 CPU 时间为：
\begin{itemize}
\item 对于 \textit{logDet}：0.069 秒，
\item 对于 \textit{minEig}：0.195 秒。
\end{itemize}

尽管这些计算时间对于实际应用来说可能已经足够，但仍然存在进一步加速计算的空间。关于这些优化的进一步探讨，我们将在第 VI 节中详细说明。

---