FAST-LIVO2论文翻译

---

摘要

代码：代码
原文：原文

摘要—本文提出了FAST-LIVO2：一个快速、直接的激光雷达-惯性-视觉里程计框架，旨在实现准确且稳健的状态估计，以支持同时定位与地图构建（SLAM）任务，并为实时的车载机器人应用提供巨大潜力。FAST-LIVO2通过误差状态迭代卡尔曼滤波器（ESIKF）高效地融合IMU、激光雷达和图像测量。为了解决异构激光雷达和图像测量之间的维度不匹配问题，我们在卡尔曼滤波器中采用顺序更新策略。为了提高效率，我们为视觉和激光雷达融合使用直接方法，其中激光雷达模块在不提取边缘或平面特征的情况下注册原始点，而视觉模块在不提取ORB或FAST角点特征的情况下最小化直接光度误差。视觉和激光雷达测量的融合基于单一统一的体素地图，其中激光雷达模块构建几何结构以注册新的激光雷达扫描，视觉模块将图像块附加到激光雷达点（即视觉地图点），从而实现新的图像对齐。为了提高图像对齐的准确性，我们利用体素地图中的激光雷达点的平面先验（甚至在对齐过程中细化平面先验），并在新的图像对齐后动态更新参考块。此外，为了增强图像对齐的稳健性，FAST-LIVO2采用按需光线投射操作，并实时估计图像曝光时间。我们在基准数据集和私有数据集上进行了广泛的实验，证明了我们提出的系统在准确性、鲁棒性和计算效率方面显著优于其他最先进的里程计系统。此外，系统中关键模块的有效性也得到了验证。最后，我们详细介绍了FAST-LIVO2的三个应用：无人机车载导航，展示了该系统在实时车载导航中的计算效率；空中测绘，展示了系统的测绘精度；以及3D模型渲染（基于网格和基于NeRF），强调了我们重建的密集地图在后续渲染任务中的适用性。我们在GitHub上开源了此工作的代码、数据集和应用，以惠及机器人社区。

图1：FAST-LIVO2实时生成的测绘结果。（a）-（c）展示了空中测绘，（d）表示使用手持设备收集的零售街道，（e）展示了一次实验，其中无人机携带激光雷达、相机和惯性传感器在其机载计算机上执行实时状态估计（即FAST-LIVO2）、轨迹规划和跟踪控制。在（d）-（e）中，蓝色线条表示计算出的轨迹。在（e1）-（e4）中，白色点表示此时的激光雷达扫描，彩色线条描绘了规划轨迹。（e1）和（e4）标记了激光雷达退化的区域。（e2）和（e3）展示了避障情况。（e5）和（e6）显示了从室内到室外的相机第一人称视角，强调了从突然过度曝光到正常的光照变化。

一、介绍

近年来，同时定位与地图构建（SLAM）特别是在未知环境中的实时3D重建和定位方面取得了显著进展。由于SLAM能够实时估计姿态并重建地图，它已成为各种机器人导航任务中不可或缺的技术。定位过程为机器人的载板控制器提供关键的状态反馈，而密集的3D地图提供了关键的环境信息，如自由空间和障碍物，这对于有效的轨迹规划至关重要。彩色地图还携带了大量的语义信息，使得真实世界的生动表现成为可能，这为虚拟现实、增强现实、3D建模和机器人-人交互等应用开辟了广阔的潜力。目前，已有几种SLAM框架使用单测量传感器成功实现，主要是相机[1]-[4]或激光雷达[5]-[7]。尽管视觉和激光雷达SLAM在各自的领域都显示出了前景，但每种都有固有的限制，限制了它们在各种场景中的性能。视觉SLAM利用成本效益高的CMOS传感器和镜头，能够建立准确的数据关联，从而实现一定程度的定位精度。丰富的颜色信息进一步丰富了语义感知。进一步利用这种增强的场景理解，深度学习方法被用于鲁棒的特征提取和动态对象过滤。然而，视觉SLAM缺乏直接的深度测量，需要通过三角测量或深度过滤等操作同时优化地图点，这引入了显著的计算开销，通常限制了地图的精度和密度。视觉SLAM还遇到了许多其他限制，如不同尺度的测量噪声变化，对光照变化的敏感性，以及无纹理环境对数据关联的影响。激光雷达SLAM利用激光雷达传感器直接获得精确的深度测量，与视觉SLAM相比，在定位和制图任务中提供了更高的精度和效率。尽管有这些优点，激光雷达SLAM也表现出几个显著的短板。一方面，它重建的点云地图虽然详细，但缺乏颜色信息，从而降低了其信息规模。另一方面，激光雷达SLAM的性能在呈现不足的几何约束的环境中趋于恶化，如狭窄的隧道、单一且延伸的墙壁等。随着在现实世界中操作智能机器人的需求增长，特别是在经常缺乏结构或纹理的环境中，越来越明显的是，依赖单一传感器的现有系统无法提供所需的准确和鲁棒的姿态估计。为了解决这个问题，融合常用的传感器，如激光雷达、相机和IMU，正日益受到关注。这一策略不仅结合了这些传感器的优势，提供了增强的姿态估计，而且还有助于构建准确、密集且彩色的点云地图，即使在单个传感器性能退化的环境中也是如此。高效准确的激光雷达-惯性-视觉里程计（LIVO）和制图仍然是具有挑战性的问题：1) 整个LIVO系统的任务是处理激光雷达测量值，每秒包含数百到数千个点，以及高比率、高分辨率的图像。充分利用如此大量的数据，特别是有限的载板资源，需要非凡的计算效率；2) 许多现有系统通常包含一个激光雷达-惯性里程计（LIO）子系统和一个视觉-惯性里程计（VIO）子系统，每个子系统都需要从视觉和激光雷达数据中提取特征，以减少计算负荷。在缺乏结构或纹理的环境中，这种提取过程通常导致有限的特征点。此外，为了优化特征提取，需要大量的工程适应性，以适应激光雷达扫描模式和点密度的变化；3) 为了减少计算需求并实现相机和激光雷达测量之间的更紧密集成，需要一个统一的地图来同时管理稀疏点和观察到的高分辨率图像测量。然而，考虑到激光雷达和相机的异构测量，设计和维护这样的地图特别具有挑战性；4) 为了确保重建的彩色点云的精度，姿态估计需要实现像素级精度。达到这一标准面临相当的挑战：适当的硬件同步、激光雷达和相机之间外部参数的严格预校准、曝光时间的精确恢复，以及能够实时达到像素级精度的融合策略。
基于这些问题，我们提出了FAST-LIVO2，这是一个高效率的LIVO系统，通过顺序更新的误差状态迭代卡尔曼滤波器（ESIKF），紧密集成了激光雷达、图像和IMU测量。在IMU传播的先验下，系统状态首先通过激光雷达测量更新，然后通过图像测量更新，两者都利用基于单一统一体素地图的直接方法。具体来说，在激光雷达更新中，系统将原始点注册到地图上，以构建和更新其几何结构；而在视觉更新中，系统直接将激光雷达地图点重用为视觉地图点，无需从图像中提取、三角测量或优化任何视觉特征。地图中选择的视觉地图点附有先前观察到的参考图像块，然后投影到当前图像上，通过最小化直接光度误差（即稀疏图像对齐）来对齐其姿态。为了提高图像对齐的准确性，FAST-LIVO2 动态更新参考块，并使用从激光雷达点获得的平面先验。为了提高计算效率，FAST-LIVO2 使用激光雷达点识别当前图像中可见的视觉地图点，并在没有激光雷达点的情况下进行按需体素光线投射。FAST-LIVO2 还实时估计曝光时间，以应对照明变化。
FAST-LIVO2 的开发基于我们之前工作中首次提出的 FAST-LIVO [8]。与 FAST-LIVO 相比，新贡献如下：

• 我们提出了一种高效的 ESIKF 框架，采用序列更新来解决激光雷达和视觉测量之间的维度不匹配，提高了使用异步更新的 FAST-LIVO 的鲁棒性。
• 我们使用（甚至细化）来自激光雷达点的平面先验，以提高准确性。相比之下，FAST-LIVO 假设补丁中的所有像素共享相同的深度，这种假设显著降低了图像对齐中仿射变换的准确性。
• 我们提出了一种参考块更新策略，通过选择具有较大视差和足够纹理细节的高质量内点参考块，来提高图像对齐的准确性。FAST-LIVO 根据与当前视图的接近度选择参考块，通常导致低质量参考块，从而降低准确性。
• 我们进行在线曝光时间估计，以处理环境照明变化。FAST-LIVO 没有解决这个问题，导致在显著光照变化下图像对齐的收敛性较差。
• 我们提出了按需体素光线投射，以增强系统在激光雷达盲区造成的缺失点测量情况下的鲁棒性，这是 FAST-LIVO 中未考虑的问题。
  上述每项贡献都在综合消融研究中进行了评估，以验证其有效性。我们将提议的系统实现为实用的开源软件，经过精心优化，以支持在 Intel 和 ARM 处理器上的实时操作。该系统功能多样，支持多线激光雷达、采用非常规扫描模式的新兴固态激光雷达，以及针孔相机和各种鱼眼相机。
  此外，我们在 25 个公共数据集（即 Hilti 和 NTU-VIRAL 数据集）序列上进行了广泛实验，并与各种具有代表性的私有数据集进行了比较，使得可以与其他先进的 SLAM 系统（例如 R3LIVE、LVI-SAM、FAST-LIO2 等）进行比较。定性和定量结果表明，我们提出的系统在准确性和鲁棒性方面显著超过其他对手，且计算成本降低。为了进一步强调我们系统的现实应用和多功能性，我们部署了三种不同的应用。首先，完全自主的 UAV 机载导航，展示了系统的实时能力，标志着使用激光雷达-惯性-视觉系统进行现实世界自主 UAV 飞行的开创性实例。其次，空中制图展示了系统在实际应用中在无结构环境下的像素级精度。最后，高质量的网格、纹理和 NeRF 模型生成强调了该系统在渲染任务中的适用性。我们在 GitHub 上提供我们的代码和数据集。

二、相关工作

A.直接方法

在视觉和激光雷达（LiDAR）SLAM中的姿态估计。与需要提取显著特征点（例如，图像中的角点和边缘像素；激光雷达扫描中的平面和边缘点）并生成鲁棒描述符以进行匹配的特征基方法 [5, 6, 9, 10] 不同，直接方法直接利用原始测量数据通过最小化基于光度误差或点到平面残差的误差函数 [11] 来优化传感器姿态，例如 [3, 12][14]。通过消除耗时的特征提取和匹配，直接方法提供了快速的姿态估计。然而，缺乏特征匹配要求相当准确的状态先验估计，以避免陷入局部最小值。视觉SLAM中的直接方法大致可分为稠密直接方法、半稠密直接方法和稀疏直接方法。稠密直接方法主要采用全深度测量的RGB-D相机，如 [15][17] 所示，应用图像到模型的对齐进行姿态估计。相对而言，半稠密直接方法 [3, 18] 通过利用灰度级梯度显著的像素实现直接图像对齐进行估计。稀疏直接方法 [2, 12] 通过仅使用少量精心选择的原始块，专注于提供准确的状态估计，从而进一步降低了与稠密和半稠密直接方法相比的计算负担。与直接视觉SLAM方法不同，直接LiDAR SLAM系统 [13, 14, 19, 20] 不区分稠密和稀疏方法，通常使用空间下采样或时间下采样的原始点在每次扫描中构建姿态优化的约束。
在我们的工作中，我们利用直接方法的原理来处理LiDAR和视觉模块。我们系统的LiDAR模块改编自VoxelMap [14]，而视觉模型基于稀疏直接方法的变体 [12]。在从 [12] 的稀疏直接图像对齐中汲取灵感的同时，我们的视觉模块通过重新利用LiDAR点作为视觉地图点，从而减轻了后端计算的负担（即特征对齐、滑动窗口优化和/或深度滤波）。

B.激光视觉惯性传感器融合

在激光雷达-视觉-惯性SLAM中集成多种传感器，使系统具备处理一系列具有挑战性环境的能力，特别是当一个传感器经历故障或部分退化时。基于此，研究界出现了各种激光雷达-视觉-惯性SLAM系统。现有方法通常可以分为两类：松耦合和紧耦合。这种分类可以从两个角度确定：状态估计层面和原始测量层面。在状态估计层面，关键是一个传感器的估计是否作为另一个传感器模型中的优化目标。在原始测量层面，涉及不同传感器的原始数据是否被结合起来。张等人提出了一个在状态估计层面松耦合的激光雷达-视觉-惯性SLAM系统[21]。在这个系统中，VIO子系统只为LIO子系统中的扫描配准提供初始姿态，而不是与扫描配准一起优化。VIL-SLAM[22]采用了类似的松耦合方法，没有利用激光雷达、相机和IMU测量的联合优化。一些系统（例如，DEMO[23]、LIMO[24]、CamVox[25]、[26]）使用3D激光雷达点为视觉模块[1, 4, 27]提供深度测量。虽然这些系统在测量层面上表现出紧密耦合，但它们在状态估计上仍然是松耦合的，主要是因为缺乏直接从激光雷达测量中派生的约束。另一个问题是，由于分辨率不匹配，3D激光雷达点没有与2D图像特征点和/或线条一一对应，这需要在深度关联中进行插值，引入了潜在的误差。为了解决这个问题，DVL-SLAM[28]采用了一种直接方法进行视觉跟踪，将激光雷达点直接投影到图像中，以确定相应像素位置的深度。上述工作没有在状态估计层面实现紧密耦合。为了追求更高的精度和鲁棒性，许多最近的研究以紧密耦合的方式联合优化传感器数据。例如，LIC-Fusion[29]基于MSCKF[30]框架，紧密融合了IMU测量、稀疏视觉特征和激光雷达平面及边缘特征。随后的LIC-Fusion2.0[31]通过在滑动窗口内实现平面特征跟踪来增强激光雷达姿态估计。VILENS[32]通过统一因子图联合优化视觉、激光雷达和惯性数据，依赖于固定滞后平滑。R2LIVE[33]在流形迭代卡尔曼滤波器[34]中紧密融合了激光雷达、相机和IMU测量。对于R2LIVE中的VIO子系统，使用滑动窗口优化来三角化地图中视觉特征的位置。有几个系统在测量和状态估计层面都实现了完全紧密耦合。LVI-SAM[35]在一个建立在因子图之上的紧密耦合平滑和映射框架中融合了激光雷达、视觉和惯性传感器。VIO子系统执行视觉特征跟踪，并使用激光雷达扫描提取特征深度。R3LIVE[36]通过LIO构建全局地图的几何结构，并通过VIO渲染地图纹理。这两个子系统通过融合各自的激光雷达或视觉数据与IMU来共同估计系统状态。高级版本R3LIVE++[37]实时估计曝光时间，并预先进行光度校准[38]，使系统能够恢复地图点的辐射度。与大多数先前提到的依赖于LIO和VIO子系统的基于特征的方法的激光雷达-惯性视觉系统不同，R3LIVE系列[36, 37]采用了直接方法，无需特征提取，使它们能够在无纹理或无结构的场景中捕捉微妙的环境特征。
我们的系统还使用激光雷达、图像和IMU数据联合估计状态，并在测量层面保持紧密耦合的体素地图。此外，我们的系统使用直接方法，利用原始激光雷达点进行激光雷达扫描配准，并使用原始图像块进行视觉跟踪。我们的系统与R3LIVE（或R3LIVE++）的关键区别在于，R3LIVE（和R3LIVE++）在VIO中在单个像素级别上操作，而我们的系统在图像块级别上操作。这种差异赋予了我们的系统显著的优势。首先，在鲁棒性方面，我们的方法使用简化的单步帧到地图稀疏图像对齐进行姿态估计，减轻了对R3LIVE中必须通过帧到帧光流获得的精确初始状态的依赖。因此，我们的系统简化并改进了R3LIVE中的两阶段帧到帧和帧到地图操作。其次，从计算角度来看，R3LIVE中的VIO主要采用密集的直接方法，这在计算上是昂贵的，需要大量的点进行残差构建和渲染。相比之下，我们的稀疏直接方法提供了增强的计算效率。最后，我们的系统利用原始图像块的分辨率信息，而R3LIVE受限于其点图的分辨率。
我们系统的可视化模块与DVLOAM[39]、SDV-LOAM[40]和LVIO-Fusion[41]最为相似，它们将附有块的激光雷达点投影到新图像中，并通过最小化直接光度误差来跟踪图像。然而，它们有几个关键区别，如使用视觉和激光雷达的单独地图，依赖于视觉模块中块变形中恒定深度的假设，状态估计层面的松耦合，以及用于图像对齐的两阶段帧到帧和帧到关键帧。相比之下，我们的系统在迭代卡尔曼滤波器中紧密集成了帧到地图图像对齐、激光雷达扫描配准和IMU测量。此外，由于激光雷达和视觉模块的单一统一地图，我们的系统可以直接使用激光雷达点提供的平面先验来加速图像对齐。

三、系统方法

我们的系统概述如图 2 所示，包含四个部分：ESIKF（第四节）、局部地图（第五节）、激光雷达测量模型（第六节）和视觉测量模型（第七节）。
异步采样的激光雷达点首先通过扫描重组，在相机的采样时间内重新组合为扫描。然后，我们通过采用序列状态更新的 ESIKF，将激光雷达、图像和惯性测量紧密耦合，在此过程中，系统状态首先通过激光雷达测量更新，然后再通过图像测量更新，均利用基于单一统一体素地图的直接方法（第四节）。为了在 ESIKF 更新中构建激光雷达测量模型（第六节），我们计算帧到点到平面的残差。为了建立视觉测量模型（第七节），我们从地图中提取当前视场内的视觉地图点，利用可见体素查询和按需光线投射；提取后，我们识别并丢弃异常的视觉地图点（例如，被遮挡或表现出深度不连续性的点）；然后，我们计算用于视觉更新的帧到地图图像光度误差。
用于视觉和激光雷达更新的局部地图是一个体素地图结构（第五节）：激光雷达点构建和更新地图的几何结构，而视觉图像则将图像块附加到选定的地图点（即视觉地图点），并动态更新参考块。更新的参考块在一个单独的线程中进一步优化其法向量。

四、带有顺序状态更新的误差状态迭代卡尔曼滤波器

本节概述了基于顺序更新的误差状态迭代卡尔曼滤波器（ESIKF）框架的系统架构。

A. 符号和状态转移模型

在我们的系统中，我们假设三个传感器（激光雷达、IMU和相机）之间的时间偏移是已知的，可以在事先校准或同步。我们以IMU帧（表示为I）作为机体帧，并将第一个机体帧作为全局帧（表示为G）。此外，我们假设三个传感器刚性连接，并且外部参数如表I中定义，已预先校准。然后，在第i次IMU测量的离散状态转移模型为：

$$x_{i+1}=x_i⊞\left(\Delta tf\left(x_i,u_i,w_i\right)\right)(1)$$

其中 $\Delta t$ 是IMU采样周期，状态 $x$、输入 $u$、过程噪声 $w$ 和函数 $f$ 定义如下：

$$\begin{array}{rl}& \mathcal{M}\triangleq SO(3)\times R^{16},\dim (\mathcal{M})=19\\ & x\triangleq {\left[\begin{array}{lllllll}{}^G{R}_I^T& {}^G{p}_I^T& {}^G{v}_I^T& b_g^T& b_a^T& {}^G{g}^T& \tau \end{array}\right]}^T\in \mathcal{M}\\ & u\triangleq {\left[\begin{array}{ll}{\omega }_m^T& a_m^T\end{array}\right]}^T,w\triangleq {\left[\begin{array}{lllll}{n}_g^T& n_a^T& n_{b_g}^T& n_{b_a}^T& n_{\tau }\end{array}\right]}^T\\ & f(x,u,w)=\left[\begin{array}{c}{\omega }_m-b_g-n_g\\ {}^G{v}_I+\frac{1}{2}\left({}^G{R}_I\left(a_m-b_a-n_a\right)+{}^{G}g\right)\Delta t\\ {}^G{R}_I\left(a_m-b_a-n_a\right)+{}^{G}g\\ n_{b_g}\\ n_{b_a}\\ 0_{3\times 1}\\ n_{\tau }\end{array}\right](2)\end{array}$$

其中 ${}^G{R}_I$、${}^G{p}_I$ 和 ${}^G{v}_I$ 分别表示IMU在全局帧中的姿态、位置和速度，${}^{G}g$ 是全局帧中的重力向量，$\tau$ 是相对于第一帧的相机曝光时间的倒数，$n_{\tau }$ 是将 $\tau$ 建模为随机游走的高斯噪声，${\omega }_m$ 和 $a_m$ 是原始IMU测量值，$n_g$ 和 $n_a$ 分别是 ${\omega }_m$ 和 $a_m$ 中的测量噪声，$b_a$ 和 $b_g$ 是IMU偏差，分别被建模为由高斯噪声 $n_{b_g}$ 和 $n_{b_a}$ 驱动的随机游走。

B. 扫描重组

我们采用扫描重组技术，将高频、顺序采样的激光雷达原始点分割成相机采样时刻的独立激光雷达扫描，如图3所示。这确保了相机和激光雷达数据在同一频率（例如，$10Hz$）同步，允许同时更新状态。

C. 传播

在ESIKF框架中，状态和协方差从时间 $t_{k-1}$（接收到上一次激光雷达扫描和图像帧时）传播到时间 $t_k$（接收到当前激光雷达扫描和图像帧时）。这种前向传播通过将(1)中的进程噪声 $w_i$ 设为零，预测每个IMU输入 $u_i$ 在 $t_{k-1}$ 和 $t_k$ 期间的状态。将传播的状态记为 $\hat{x}$，协方差记为 $\hat{P}$，它将作为后续更新的先验分布。此外，为了补偿运动失真，我们进行了后向传播，确保激光雷达扫描中的点在扫描结束时间 $t_k$ 被“测量”。注意，为了简化符号，我们在所有状态向量中省略了下标 k。

D. 顺序更新

IMU传播的状态 $\hat{x}$ 和协方差 $\hat{P}$ 为时间 $t_k$ 的系统状态 x 提供了先验分布，如下所示：

$$x\boxminus \hat{x}\sim \mathcal{N}(0,\hat{P})(3)$$

我们将上述先验分布记为 $p(x)$，激光雷达和相机的测量模型为：

$$\left[\begin{array}{l}{y}_l\\ y_c\end{array}\right]=\left[\begin{array}{l}{h}_l\left(x,v_l\right)\\ h_c\left(x,v_c\right)\end{array}\right](4)$$

其中 $v_l\sim \mathcal{N}\left(0,{\Sigma }_{v_l}\right)$ 和 $v_c\sim \mathcal{N}\left(0,{\Sigma }_{v_c}\right)$ 分别表示激光雷达和相机的测量噪声。
标准的ESIKF[43]会使用所有当前测量值，包括激光雷达测量 $y_l$ 和图像测量 $y_c$，来更新状态 x。然而，激光雷达和图像测量是两种不同的传感方式，其数据维度不匹配。此外，图像测量的融合可能在图像金字塔的不同层级进行。为了解决维度不匹配问题，并为每个模块提供更多的灵活性，我们提出了一种顺序更新策略。这种策略在假设激光雷达测量 $y_l$ 和图像测量 $y_c$ 在给定状态向量 x 的情况下统计独立（即，测量被统计独立的噪声污染）时，理论上等同于使用所有测量值的标准更新。
为了引入顺序更新，我们将当前状态 x 的总条件分布重写为：

$$\begin{array}{rl}p\left(x\mid y_l,y_c\right)& \propto p\left(x,y_l,y_c\right)=p\left(y_c\mid x,y_l\right)p\left(x,y_l\right)\\ & =p\left(y_c\mid x\right)\underset{\propto p\left(x\mid y_l\right)}{\underbrace{p\left(y_l\mid x\right)p(x)}}\end{array}(5)$$

方程(5)意味着总条件分布 $p\left(x\mid y_l,y_c\right)$ 可以通过两个顺序贝叶斯更新获得。第一步只将激光雷达测量 $y_l$ 与IMU传播的先验分布 $p(x)$ 融合，以获得分布 $p\left(x\mid y_l\right)$：

$$p\left(x\mid y_l\right)\propto p\left(y_l\mid x\right)p(x)(6)$$

第二步然后将相机测量 $y_c$ 与 $p\left(x\mid y_l\right)$ 融合，以获得 x 的最终后验分布：

$$p\left(x\mid y_l,y_c\right)\propto p\left(y_c\mid x\right)p\left(x\mid y_l\right)(7)$$

有趣的是，(6) 和(7)中的两次融合遵循相同的形式：

$$q(x\mid y)\propto q(y\mid x)q(x)(8)$$

为了进行(8)中的融合，无论是激光雷达还是图像测量，我们详细说明先验分布 $q(x)$ 和测量模型 $q(y\mid x)$ 如下。对于先验分布 $q(x)$，将其记为 $x=\hat{x}⊞\delta x$ 其中 $\delta x\sim \mathcal{N}(0,\hat{P})$。在激光雷达更新的情况下（即，第一步），$(\hat{x},\hat{P})$ 是从传播步骤中获得的状态和协方差。在视觉更新的情况下（即，第二步），$(\hat{x},\hat{P})$ 是从激光雷达更新中获得的收敛状态和协方差。

为了获得测量模型分布 $q(y\mid x)$，将状态估计在第 $\kappa$ 次迭代时记为 ${\hat{x}}^{\kappa }$，其中 ${\hat{x}}^0=\hat{x}$。在 ${\hat{x}}^{\kappa }$ 处对测量模型(4)（无论是激光雷达还是相机测量）进行一阶泰勒展开，得到：

$$y|x\simeq \underset{z^{\kappa }}{\underbrace{h({\hat{x}}^{\kappa },0)}}+H^{\kappa }\delta x^{\kappa }+L^{\kappa }v(9)$$

$$q(y|x)\simeq \mathcal{N}(h({\hat{x}}^{\kappa },0)+H^{\kappa }\delta x^{\kappa },R)(10)$$

其中 $\delta x^{\kappa }=x\boxminus {\hat{x}}^{\kappa }$，$z^{\kappa }$ 是残差，$L^{\kappa }v\sim \mathcal{N}(0,R)$ 是合并测量噪声，$H^{\kappa }$ 和 $L^{\kappa }$ 是 $h\left({\hat{x}}^{\kappa }⊞\delta x^{\kappa },v\right)$ 对 $\delta x^{\kappa }$ 和 v 的雅可比矩阵，分别在零点处求值。然后，将先验分布 $q(x)$ 和测量分布 $q(y\mid x)$ 在(10)中代入后验分布(8)并执行最大似然估计(MLE)，我们可以从ESIKF框架[43]中的标准更新步骤获得 $\delta x^{\kappa }$（因此 $x^{\kappa }$）的最大后验估计(MAP)：

$$K={\left({\left(H^{\kappa }\right)}^T{R}^{-1}{H}^{\kappa }+{\hat{P}}^{-1}\right)}^{-1}{\left(H^{\kappa }\right)}^T{R}^{-1},(11)$$

$${\hat{x}}^{\kappa +1}={\hat{x}}^{\kappa }⊞\left(-K{z}^{\kappa }-\left(I-K{H}^{\kappa }\right)\left({\hat{x}}^{\kappa }\boxminus \hat{x}\right)\right)$$

然后收敛的状态和协方差矩阵构成了后验分布 $q(x\mid y)$ 的均值和协方差。带有顺序更新的卡尔曼滤波器已在文献中进行了研究，例如在[44, 45]中。本文采用这种方法对激光雷达和相机系统的ESIKF进行研究。带有顺序更新的ESIKF的实现细节在算法1中有详细说明。在第一步（第6-10行）中，误差状态从激光雷达测量（第VI-A节）迭代更新直到收敛。再次表示为 $\hat{x}$ 和 $\hat{P}$ 的收敛状态和协方差估计用于更新地图的几何结构（第V-B节），然后在第二步视觉更新（第13-23行）中，在图像金字塔（第VII-B节）的每个层级上进行细化，直到收敛。最优状态和协方差，表示为 $\overline{x}$ 和 $\overline{P}$，用于传播传入的IMU测量（第IV-C节）和更新地图的视觉结构（第V-D节和V-E节）。

五、地图

图 4：局部地图滑动的 2D 演示。在 (a) 中，灰色矩形是初始地图区域，长度为 L。红色圆圈是以 p0 为中心的初始检测区域。在 (b) 中，检测区域移动到新的位置 p1，此时触及地图边界。地图区域通过距离 d 移动到新位置（蓝色矩形）。在 © 中，内存空间 B 保持不变。存储绿色区域的内存空间 A 被重置为 (b) 中的蓝色区域 C。

A. 地图结构

我们的地图采用了[14]中提出的自适应体素结构，由哈希表和每个哈希条目的八叉树组织而成（图2）。哈希表管理根体素，每个根体素具有固定的尺寸 $0.5\times 0.5\times 0.5$ 米。每个根体素包含一个八叉树结构，以进一步组织不同大小的叶子体素。叶子体素表示一个本地平面，并存储平面特征（即，平面中心、法线向量和不确定性）以及一组位于该平面上的激光雷达原始点。这些点中的一些附有三级图像块 $(8\times 8$ 补丁大小)，我们称之为视觉地图点。收敛的视觉地图点只附有参考块，而非收敛的点附有参考块和其他可见块（见第V-E节）。叶子体素的大小不一，使其能够表示不同规模的本地平面，因此能够适应不同结构的环境[14]。

为了防止地图大小无限制增长，我们只保留位于激光雷达当前位置周围大局部区域 $L$ 内的一个局部地图，如图4中的2D示例所示。最初，地图是一个以激光雷达起始位置 $p_0$ 为中心的立方体。激光雷达的检测区域被可视化为一个以当前位置为中心的球体，其半径由激光雷达的检测范围定义。当激光雷达移动到新位置 $p_1$，检测区域触及地图边界时，我们通过距离 $d$ 将地图从边界移开。随着地图的移动，包含在局部地图外移出的区域的内存将被重置，以存储新移入局部地图的区域。这种环形缓冲区方法确保我们的局部地图在固定大小的内存内维护。环形缓冲区哈希图的实现细节在[46]中有详细说明。在每个ESIKF更新步骤后执行地图移动检查。

B. 几何结构的构建和更新

地图的几何结构是由激光雷达点测量构建和更新的。具体来说，在ESIKF中的激光雷达更新（第IV节）之后，我们将所有来自激光雷达扫描的点注册到全局框架中。对于每个注册的激光雷达点，我们确定其在哈希图中所在的根体素。如果体素不存在，则我们使用新点初始化体素并将其索引到哈希图中。如果确定的体素已存在于地图中，我们将点追加到现有的体素中。在所有扫描点分配完毕后，我们按如下方式进行几何结构的构建和更新。

对于新创建的体素，我们根据奇异值分解确定所包含的点是否位于平面上。如果是，我们计算中心点 $q=\overline{p}$、平面法线 $n$，以及 $(q,n)$ 的协方差矩阵 ${\Sigma }_{n,q}$。${\Sigma }_{n,q}$ 用于描述平面不确定性，该不确定性来自于姿态估计不确定性和点测量噪声。详细的平面标准和平面参数及不确定性的计算可以参考我们之前的工作[14]。如果所包含的点不位于平面上，体素将被不断细分为八个更小的八分体，直到子体素中的点被确定为形成平面或达到最大层级（例如，3）。在后一种情况下，叶子体素中的点将被丢弃。因此，地图只包含被确定为平面的体素（无论是根体素还是子体素。
对于有新点追加的现有体素，我们评估新点是否仍然与根体素或子体素中的现有点形成平面。如果不是，我们如上所述进行体素细分。如果是，我们也如上所述更新平面参数(q,n)和协方差 En,q。一旦平面参数收敛（见[14]），平面将被该平面将被视为完成，并且在此平面上的新点将被丢弃。此外，成熟平面的估计平面参数（q,n）和协方差 ${\Sigma }_{n,q}$ 将被固定。在平面上（无论是根体素还是子体素）的激光雷达点将用于生成后续部分中的视觉地图点。对于成熟的平面，最近的50个激光雷达点是视觉地图点生成的候选点，而对于未成熟的平面，则所有激光雷达点都是候选点。视觉地图点生成过程将从这些候选点中识别出一些视觉地图点，并将它们附加上图像块以进行图像对齐。

C. 视觉地图点的生成与更新

为了生成和更新视觉地图点，我们选择地图中的候选激光雷达点，这些点（1）在当前帧中可见（详见第VII-A节），以及（2）在当前图像中显示出显著的灰度梯度。我们将这些候选点在视觉更新后（第IV-D节）投影到当前图像上，并保留每个体素中局部平面深度最小的候选点。然后，我们将当前图像划分为每个30 x 30像素的均匀网格单元。如果一个网格单元中没有投影在此的视觉地图点，我们使用灰度梯度最高的候选点生成一个新的视觉地图点，并将其与当前图像块、估计的当前状态（即，帧姿态和曝光时间）以及从上一节中计算出的激光雷达点的平面法线相关联。附加到视觉地图点的图像块有三个相同大小的层（例如，11x11像素），每个层都是从前一个层减半采样，形成一个图像块金字塔。如果一个网格单元包含投影在此的视觉地图点，我们如果满足以下条件，则向现有视觉地图点添加新的图像块（所有三个层的金字塔）：（1）自从其最后一次添加图像块以来已超过20帧，或者（2）其在当前帧中的像素位置与其在最后一次添加图像块时的位置偏离超过40像素。因此，地图点将可能具有具有均匀分布视角的有效图像块。除了图像块金字塔外，我们还附加了估计的当前状态（即，姿态和曝光时间）到地图点。

D. 参考块更新

由于添加了新的图像块，一个视觉地图点可能有多于一个的图像块。我们需要选择一个参考图像块用于视觉更新中的图像对齐。具体来说，我们根据光度相似性和视图角度为每个图像块 f 打分如下：
$$\begin{array}{rl}NCC(f,g)& =\frac{{\sum }_{x,y}[f(x,y)-\overline{f}][g(x,y)-\overline{g}]}{\sqrt{{\sum }_{x,y}[f(x,y)-\overline{f}{]}^2{\sum }_{x,y}[g(x,y)-\overline{g}{]}^2}}\\ c& =\frac{n\cdot p}{\Vert p\Vert },{\omega }_1=\frac{1}{1+e^{tr\left({\Sigma }_n\right)}}\\ S& =\left(1-{\omega }_1\right)\cdot \frac{1}{n}\sum _{i=1}^{n}NCC\left(f,g_i\right)+{\omega }_1\cdot c(12)\end{array}$$
其中 $NCC(f,g)$ 表示用于测量图像块 f 和 g 在第0层金字塔（具有最高分辨率的层）的相似度的归一化互相关（NCC），对两个图像块都应用了均值减法，c 表示正在评估的图像块 f 的法线向量 n 和视图方向 p/||p|| 之间的余弦相似度。当图像块直接面向地图点所在平面时，c 的值为1。总评分 S 通过加权 NCC 和 c 计算得出，前者表示正在评估的图像块 f 和所有其他图像块 $g_i$ 之间的平均相似度，$tr({\Sigma }_n)$ 表示法线向量的协方差矩阵的迹。在附加到视觉地图点的所有图像块中，得分最高的图像块被更新为参考图像块。上述评分机制倾向于选择（1）外观与大多数其他图像块相似（以NCC为依据）的参考图像块，MVS[47]中使用的技术，用于避免动态对象上的图像块；（2）视图方向与平面垂直，从而在高分辨率下保持纹理细节。相比之下，我们之前工作中的参考图像块更新策略 FAST-LIVO[8] 和先前研究[4]直接选择与当前帧视图方向差异最小的图像块，导致所选的参考图像块非常接近当前帧，因此对当前姿态更新施加的约束较弱。

图5：(a) 参考图像块和目标参考框架图像块之间的仿射变形。(b) 任何位于归一化球体上的法线 Irn∈ S2 首先被投影到平面 $I_r$ 上的点 M $\in R^3$，使得 $p^{T}M=1$，然后被投影到 x-y 平面上的点 $m\in R^2$。这种转换因此将球体上的扰动转换为 x-y 平面上的扰动 dm。

E. 法线细化

每个视觉地图点被假设位于一个小的局部平面上。现有工作[2, 4, 8]假设图像块中的所有像素具有相同的深度，这是一个通常不成立的大胆假设。我们使用第V-B节中详细说明的从激光雷达点计算出的平面参数，以实现更高的精度。这个平面法线对于执行视觉更新过程中图像对齐的仿射变形至关重要。为了进一步提高仿射变形的精度，可以从附加到视觉地图点的图像块中进一步细化平面法线。具体来说，我们通过最小化与其他附加到视觉地图点的图像块的光度误差来细化参考图像块中的平面法线。

1. 仿射变形：仿射变形用于将参考框架中的图像块像素（即，源图像块）转换为其他框架中的图像块像素（即，目标图像块），如图5(a)所示。设 $u_j^r$为源图像补丁中的第 $j$ 个像素坐标，$u_j^i$ 为第$i$ 个目标图像补丁中的第 $j$ 个像素坐标。假设图像块中的所有像素都位于具有法线 ${}^{I_r}n$ 和视觉地图点位置 ${}^{I_r}p$ 的局部平面上（这对应于源图像块和目标图像块的中心像素），两者都在源图像块框架中表示，我们有：

$$\begin{array}{rl}{u}_i^j& =A_r^i{u}_r^j\\ A_r^i& =P\left({}^{I_i}{R}_{I_r}+{}^{I_i}{t}_{I_r}\frac{1}{I_r{n}^T\cdot I_{r}p}{}^{I_r}{n}^T\right)P^{-1}(13)\end{array}$$

其中 $A_r^i$ 表示将像素坐标从源（或参考）图像块转换为第 i 个目标图像块的仿射变形矩阵，${}^{I_i}{R}_{I_r}$ 和 ${}^{I_i}{t}_{I_r}$ 表示参考框架 $I_r$ 相对于目标框架 $I_i$ 的相对姿态。为了直接使用鱼眼图像而不将它们校正为针孔图像，我们根据不同的相机模型实现了投影矩阵 P 和反投影矩阵 $P^{-1}$（例如，对于针孔相机模型，P 是相机的内在矩阵）。
2) 法线优化：为了细化平面法线 ${}^{I_r}n$，我们最小化参考图像块和其他图像块在第 0 层金字塔（即，最高分辨率层）之间的光度误差：

$$I_r{n}^{\ast }=\arg \underset{I_{r}n\in S^2}{\min }\sum _{i\in S}\sum _{j=1}^{N^2}{\Vert {\tau }_i{I}_i\left(A_r^i{u}_r^j\right)-{\tau }_r{I}_r\left(u_r^j\right)\Vert }_2\text{(14)}$$

其中 N 是路径大小，${\tau }_r$ 和 ${\tau }_i$ 分别是参考框架和第 i 个目标框架的倒数曝光时间。$I_r\left(u_r^j\right)$ 表示参考框架中的第 j 个图像块像素，$I_i\left(A_r^i{u}_r^j\right)$ 表示第 i 个目标框架中的第 j 个图像块像素，S 是所有目标框架的集合。

3. 优化变量变换：为了提高计算效率，我们重新参数化了(14)中的最小二乘问题。注意，优化变量 ${}^{I_r}n$ 只出现在 $M\triangleq \frac{1}{I_r{n}^T\cdot I_{r}p}$ ${}^{I_r}n\in R^3$ 中的(13)，可以对 M 进行 ${}^{I_r}n$ 的优化。此外，向量 M 受到约束 ${}^{I_r}p\cdot M=1$，意味着 M 可以如下参数化：

$$\begin{array}{rl}& M=\left[\begin{array}{cc}\frac{M_x}{I_r{p}_z}-\frac{I_r{p}_x}{I_r{p}_z}{M}_x-\frac{I_r{p}_y}{I_r{p}_z}{M}_y& \\ 1& 0\\ 0& 1\\ -\frac{I_r{p}_x}{I_r{p}_z}& -\frac{I_r{p}_y}{I_r{p}_z}\end{array}\right],b=\left[\begin{array}{c}0\\ 0\\ \frac{1}{I_r{p}_z}\end{array}\right],m=\left[\begin{array}{c}{M}_x\\ M_y\end{array}\right]\in R^2\end{array}(15)$$

其中 ${}^{I_r}{p}_z\ne 0$，因为没有这样的参考图像块可以被选择用于视觉地图点。图 5(b) 显示了 ${}^{I_r}n$、M 和 m 之间的关系。

最后，(14)中的优化是在向量 $m\in R^2$ 上进行的，没有任何约束。这种优化可以在单独的线程中进行，以避免阻塞主里程计线程。然后可以使用优化参数 $m^{\ast }$ 来恢复最优法线向量 ${}^{I_r}{n}^{\ast }$：

$${}^{I_r}{n}^{\ast }=\frac{M^{\ast }}{\Vert M^{\ast }\Vert },M^{\ast }=B{m}^{\ast }+b(16)$$

一旦平面法线收敛，该视觉地图点的参考图像块和法线向量将被固定，不再进行进一步的细化，所有其他图像块将被删除。

六、激光雷达测量模型

本节详细描述了第IV-D节中ESIKF激光雷达更新使用的激光雷达测量模型 $y_l=h_l\left(x,v_l\right)$。

A. 点对平面激光雷达测量模型

在获得扫描中的未畸变点 $\left\{{}^L{p}_j\right\}$ 后，我们使用激光雷达更新的第 $\kappa$ 次迭代中估计的状态 ${\hat{x}}^{\kappa }$ 将它们投影到全局框架：

$${}^G{\hat{p}}_j^{\kappa }={}^G{\hat{T}}_I^{\kappa I}{T}_L{}^L{p}_j(17)$$

然后我们确定 $\kappa$ 次迭代中 ${}^G{\hat{p}}_j^{\kappa }$ 所在的根体素或子体素。如果在哈希图中没有找到体素或体素不包含平面，则丢弃该点。否则，我们使用体素中的平面建立激光雷达点的测量方程。具体来说，我们假设真实的激光雷达点 ${}^L{p}_j^{gt}$，在给定精确的激光雷达姿态 ${}^G{T}_I$ 的情况下，应该位于体素中具有法线 $n_j^{gt}$ 和中心点 $q_j^{gt}$ 的平面上。即，

$$0={\left(n_j^{gt}\right)}^T\left({}^G{T}_I{}^I{T}_L{}^L{p}_j^{gt}-q_j^{gt}\right)(18)$$

由于真实的地面点 ${}^L{p}_j^{gt}$ 被测量为 ${}^L{p}_j$，并且具有测距和方位噪声 ${\delta }^L{p}_j$，我们有 ${}^L{p}_j^{gt}={}^L{p}_j-{\delta }^L{p}_j$。同样，平面参数 $\left(n_j^{gt},q_j^{gt}\right)$ 被估计为 $\left(n_j,q_j\right)$，其协方差为 ${\Sigma }_{n,q}$（第V-B节），因此我们有：$n_j^{gt}=n_j\boxminus \delta n_j,q_j^{gt}=q_j-\delta q_j$。因此，

$$\underset{y_1}{\underbrace{0}}=\underset{h_l\left(x,v_l\right)}{\underbrace{{\left(n_j\boxminus \delta n_j\right)}^T\left({}^G{T}_I{}^I{T}_L\left({}^L{p}_j-{\delta }^L{p}_j\right)-\left(q_j-\delta q_j\right)\right)}}(19)$$

其中测量噪声 $v_l=\left({\delta }^L{p}_j,\delta n_j,\delta q_j\right)$ 包括与激光雷达点、法线向量和平面中心相关的噪声。

B. 考虑光束发散的激光雷达测量噪声

在局部激光雷达框架中，激光雷达点 ${\delta }^L{p}_j$ 的不确定性被分解为两个部分[14]：由激光飞行时间（TOF）引起的测距不确定性 $\delta d$，以及源自编码器的方位方向不确定性 $\delta \omega$。除了这些不确定性外，我们还考虑了由激光束发散角 $\theta$ 引起的不确定性，如图 6 所示。随着方位方向和法线向量之间的夹角 $\phi$ 增大，激光雷达点的测距不确定性显著增加，而方位方向不确定性保持不变。由于激光束发散角引起的 $\delta d$ 可以建模为：

$$\delta d=L_2-L_1=d\left(\frac{\cos \phi }{\cos (\theta +\phi )}-\frac{\cos \phi }{\cos (\theta -\phi )}\right)(20)$$

考虑到受 TOF 和激光束发散影响的 $\delta d$，当我们的系统从地面或墙面选择更多点时（见图 6(c, d)），它实现了比不考虑这种效应时更精确的姿态估计。

图 6：(a) 和 (b) 分别展示了考虑激光束发散角度$\theta$ 的 LiDAR 点不确定性模型的 3D 和侧面剖视图。红色轮廓描绘了激光束扩散的区域。© 和 (d) 中的点按位置不确定性进行了着色。与 © 相比，(d) 进一步考虑了由发散角度导致的测距不确定性 $\delta d$，从而使地面点的不确定性更高，因为激光束的扩散区域较大。

七. 视觉测量模型

本节详细描述了第IV-D节中ESIKF视觉更新使用的视觉效果模型 $y_c=h_c(x,v_c)$。

图8：离群点剔除。(a)展示了被遮挡和深度不连续的视觉地图点的示意图。(b)展示了在真实场景中离群点剔除的效果。红色点表示被剔除的视觉地图点，而绿色点表示被接受的视觉地图点。

A. 视觉地图点选择

为了在视觉更新中执行稀疏图像对齐，我们首先选择适当的视觉地图点。我们首先提取在当前相机视场（FoV）内可见的地图点集（称为视觉子图），使用体素和光线投射查询。然后，从这个子图中选择视觉地图点并排除异常值。这个过程产生了一组准备用于在视觉测量模型中构建视觉光度误差的精炼视觉地图点。

1. 可见体素查询：由于地图中体素数量众多，确定当前帧视场内的地图体素是具有挑战性的。为了解决这个问题，我们调查了当前扫描中激光雷达点命中的体素。这可以通过使用测量点位置查询体素哈希表高效完成。如果相机视场与激光雷达视场大部分重叠，相机视场内的地图点很可能也位于这些体素中。我们还调查了在前一图像帧中通过相同体素查询和光线投射确定为可见的地图点的体素，假设两个连续的图像帧有大的视场重叠。最后，当前视觉子图可以通过这些两类体素中包含的地图点后跟视场检查获得。
2. 按需光线投射：在大多数情况下，可以通过上述体素查询获得视觉子图。然而，当激光雷达传感器过于靠近物体时（称为近场盲区），可能没有返回点。此外，相机视场可能没有被激光雷达视场完全覆盖。为了在这些情况下召回更多的视觉地图点，我们采用了如图7所示的光线投射策略。我们将图像划分为每个30×30像素的均匀网格单元，并将从体素查询中获得的视觉地图点投影到网格单元上。对于未被这些视觉地图点占据的每个图像网格单元，沿着中心像素向后投射光线，其中样本点沿光线在深度方向从 dmin 到 dmax 均匀分布。为了减少计算负荷，预先计算了每个光线上样本点在相机机体框架中的位置。对于每个样本点，我们评估相应体素的状态：如果体素包含在投影后位于这个网格单元内的视觉地图点，我们将这些地图点纳入视觉子图并停止对此光线的处理。否则，我们继续处理光线上的下一个样本点，直到达到最大深度 dmax。处理完所有未占据的图像网格单元后，我们获得了一组分布在整个图像中的视觉地图点。
3. 异常值排除：在体素查询和光线投射后，我们获得了当前帧视场内的所有视觉地图点。然而，这些视觉地图点可能在当前帧中被遮挡，深度不连续，其参考块在大视角下拍摄，或者在当前帧中有大视角，所有这些都可能严重降低图像对齐精度。为了解决第一个问题，我们使用激光雷达更新后的姿态将子图中的所有视觉地图点投影到当前帧中，并保留每个30×30像素网格单元中的最低深度点。为了解决第二个问题，我们将当前激光雷达扫描中的激光雷达点投影到当前帧中，产生深度图。通过比较视觉地图点与深度图中9×9邻域的深度，我们确定它们的遮挡和深度变化。被遮挡和深度不连续的地图点被排除（见图8）。为了解决第三和第四个问题，我们移除了参考块或当前块的视角（即，从视觉地图点到块光学中心的法线向量和方向之间的夹角）过大（例如，超过80°）的点。剩余的视觉地图点将用于对齐当前图像。
   将子图中的视觉地图点使用激光雷达更新后的姿态投影到当前帧 ti 中，并在每个 $30\times 30$ 像素的网格单元中保留最低深度点。为了解决第二个问题，我们将当前激光雷达扫描中的激光雷达点投影到当前帧中，产生深度图。通过比较视觉地图点与深度图中 $9\times 9$ 邻域的深度，我们确定它们的遮挡和深度变化。被遮挡和深度不连续的地图点被排除（见图 8）。为了解决第三和第四个问题，我们移除了参考块或当前块的视图角度（即，从视觉地图点到块光学中心的法线向量和方向之间的夹角）过大（例如，超过 $80^{\circ }$）的点。剩余的视觉地图点将用于对齐当前图像。

B. 稀疏直接视觉测量模型

上述提取的视觉地图点 $\left\{{}^G{p}_i\right\}$ 用于构建视觉测量模型。基本原理是，当将地图点 ${}^G{p}_i$ 转换到具有真实状态（即，姿态）$x_k$ 的当前图像 $I_k(\cdot )$ 时，参考块和当前块之间的光度误差应该为零：

$$\begin{array}{rl}0={\tau }_k{I}_k^{gt}& (\underset{u_i}{\underbrace{\pi ({}^C{T}_I({}^G{T}_I{)}^{-1}{}^G{p}_i)}}+\Delta u)\\ & -{\tau }_r{I}_r^{gt}(\underset{u_i^{\prime }}{\underbrace{\pi ({}^{C_r}{T}_G{}^G{p}_i)}}+A_i^r\Delta u)(21)\end{array}$$
其中 $\pi (\cdot )$ 是常见的相机投影模型（例如，针孔、MEI、ATAN、Scaramuzza、等距），${}^{C_r}{T}_G$ 是全局框架 G 相对于参考框架 $C_r$ 的姿态，这在接收和融合参考框架时已经估计，$A_i^r$ 是将像素从第 i 个当前块转换到参考块的仿射变形矩阵，$\Delta u$ 是相对于当前块中心 $u_i$ 的相对像素位置，$I_k^{gt},I_r^{gt}$ 分别表示参考和当前框架的真实像素值。它们被测量为实际的图像像素值 $I_k,I_r$，带有测量噪声 $v_c=\left(\delta I_k,\delta I_r\right)$，这些噪声源自各种来源（例如，相机 CMOS 的射击噪声和模数转换器（ADC）噪声）。因此，

$$\underset{y_c}{\underbrace{0}}=\underset{h_c(x,v_c)}{\underbrace{{\tau }_k(I_k(u_i+\Delta u)-\delta I_k)-{\tau }_r(I_r(u_i^{\prime }+A_i^r\Delta u)-\delta I_r)}}(22)$$

为了提高计算效率，我们采用了逆向合成公式[4,48]，其中姿态增量 $\delta T\in R^6$ 参数化 ${}^G{T}_I={}^G{\hat{T}}_I^{\kappa }\mathrm{Exp}(\delta T)$ 在 $u_i$ 中（见(21)），从 $u_i$ 移动到 $u_i^{\prime }$ 如下：

$$\begin{array}{rl}& u_i=\pi \left({}^C{T}_I{\left({}^G{\hat{T}}_I^{\kappa }\right)}^{-1}G{p}_i\right)\\ & u_i^{\prime }=\pi \left({}^{C_r}{T}_G\mathrm{Exp}(\delta T{)}^G{p}_i\right)(23)\end{array}$$

鉴于参考框架中的 $u_i^{\prime }$ 在每次迭代期间保持不变，我们只需要一次性计算相对于 $\delta T$ 的雅可比矩阵，而不需要在每次迭代中重新计算它们。
为了从测量方程(22)中估计逆曝光时间 ${\tau }_k$，我们将初始逆曝光时间固定为 ${\tau }_0=1$，以消除当所有逆曝光时间都为零时方程(22)的退化。因此，后续帧的估计逆曝光时间是相对于第一帧的曝光时间。
方程(22)用于三个级别的视觉更新步骤（见算法 1）；视觉更新从最粗糙的级别开始，在一级收敛后，它进入下一个更细的级别。然后使用估计的状态生成视觉地图点（第V-C节）和更新参考块（第V-D节）。

八、评估

在本节中，我们介绍了用于性能评估的数据集，包括公共数据集 NTU-VIRAL[49]、Hilti’22[50]、Hilti’23[51] 和 MARS-LVIG[52]，以及我们自收集的 FAST-LIVO2 私有数据集。具体来说，NTU-VIRAL 和 Hilti 数据集用于对我们的系统与其他最先进的（SOTA）SLAM系统（第IX-B节）进行定量基准比较。FAST-LIVO2 私有数据集主要用于在各种极具挑战性的场景中评估我们的系统（第IX-C节），展示其高精度制图能力（第IX-D节），并验证我们系统中各个模块的功能（补充材料[53]中的第I-A至I-D节）。MARS-LVIG 数据集用于应用演示（第X节）和消融研究（补充材料[53]中的第I-E节）。

图9：我们的带有硬件同步功能的数据采集平台。(a) 我们的手持平台，(b) 硬件同步方案。

结果

图10：在激光雷达退化场景中实时生成的地图结果。从上至下的点云分别对应“亮屏墙”、“黑屏墙”、“工大涂鸦墙”（第三和第四行）、“横幅墙”以及“港大教学楼”，展示了使用FAST-LIVO2、FAST-LIVO或R3LIVE构建的彩色点云的对比

图11：在复杂的激光雷达退化及视觉挑战性场景中实时生成的地图结果。(a)、(b)和©分别对应“港大文化中心”、“中央商务区03号楼”和“采矿隧道”。不同颜色的箭头表示由不同传感器引起的退化方向。