FAST-LIVO2: 快速、直接的 LiDAR-惯性-视觉里程计

Chunran Zheng¹, Wei Xu¹, Zuhao Zou¹, Tong Hua¹, Chongjian Yuan¹, Dongjiao He¹, Bingyang Zhou¹, Zheng Liu¹, Jiarong Lin¹, Fangcheng Zhu¹, Yunfan Ren¹, Rong Wang², Fanle Meng², Fu Zhang^1† ^†Corresponding author (email: fuzhang@hku.hk)¹Mechatronics and Robotic Systems (MaRS) Laboratory, Department of Mechanical Engineering, University of Hong Kong, Hong Kong SAR, China.²Information Science Academy of China Electronics Technology Group Corporation

摘要

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

索引词：

同时定位与地图构建 (SLAM)、传感器融合、3D 重建、空中导航。

I 引言

近年来，同步定位与建图 (SLAM) 技术取得了重大进展，特别是在未知环境中的实时 3D 重建和定位方面。由于 SLAM 能够实时估计姿态并重建地图，因此已成为各种机器人导航任务必不可少的工具。定位过程为机器人的车载控制器提供了重要的状态反馈，而密集的 3D 地图则提供了关键的环境信息，例如自由空间和障碍物，这些信息对于有效的轨迹规划至关重要。彩色地图还承载着大量的语义信息，能够生动地展现现实世界，从而开辟了广阔的潜在应用领域，例如虚拟现实和增强现实、3D 建模以及人机交互。

目前，一些 SLAM 框架已成功地与单一测量传感器（主要是摄像头 [1, 2, 3, 4] 或激光雷达 [5, 6, 7]）实现。虽然视觉 SLAM 和激光雷达 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是在我们之前的工作[8]中首次提出的FAST-LIVO的基础上开发的。与 FAST-LIVO 相比，新的贡献列举如下：

1.

我们提出了一种高效的 ESIKF 框架，该框架采用顺序更新来解决 LiDAR 和视觉测量之间维数不匹配的问题，从而提高了使用异步更新的 FAST-LIVO 的鲁棒性。
2.

我们使用（甚至改进）来自 LiDAR 点的平面先验以提高精度。相反，FAST-LIVO 假设一个补丁中的所有像素共享相同的深度，这是一个非常大胆的假设，它大大降低了图像对齐中仿射扭曲的精度。
3.

我们提出了一种参考补丁更新策略来提高图像对齐的精度，方法是选择具有较大视差和足够纹理细节的高质量、内点参考补丁。 FAST-LIVO 根据与当前视图的接近程度选择参考补丁，这通常会导致低质量的参考补丁降低精度。
4.

我们进行在线曝光时间估计，以处理环境照明变化。 FAST-LIVO 没有解决这个问题，导致图像对齐在显着照明变化的情况下收敛性差。
5.

我们提出按需体素射线投射，以增强系统在 LiDAR 点测量缺失时的鲁棒性，这些缺失是由 LiDAR 近距离盲区引起的，FAST-LIVO 未考虑这个问题。

上述每项贡献都在全面的消融研究中进行了评估，以验证其有效性。我们将所提出的系统实现为实用的开源软件，经过精心优化，可在英特尔和 ARM 处理器上实时运行。该系统用途广泛，支持多线旋转 LiDAR、具有非常规扫描模式的新兴固态 LiDAR，以及针孔相机和各种鱼眼相机。

此外，我们在公共数据集（例如，Hilti 和 NTU-VIRAL 数据集）的 25 个序列上进行广泛的实验，以及各种代表性的私有数据集，从而能够与其他最先进的 SLAM 系统（例如，R3LIVE、LVI-SAM、FAST-LIO2 等）进行比较。定性和定量结果表明，我们提出的系统在精度和鲁棒性方面明显优于其他同行，同时计算成本更低。

为了进一步强调我们系统的实际适用性和多功能性，我们部署了三个不同的应用程序。首先，完全机载的无人机自主导航，展示了系统的实时能力，标志着将 LiDAR-惯性-视觉系统用于现实世界自主无人机飞行的开创性案例。其次，机载测绘展示了该系统在实际使用中无结构环境下的像素级精度。最后，高质量的网格、纹理和 NeRF 模型生成突出了该系统适用于渲染任务。我们在 GitHub 上提供了我们的代码和数据集。

II 相关工作

II-A 直接方法

直接方法是视觉和 LiDAR SLAM 中快速姿态估计的突出方法。与基于特征的方法 [9, 10, 5, 6] 不同，基于特征的方法需要提取显著特征点（例如，图像中的角点和边缘像素；LiDAR 扫描中的平面和边缘点）并生成用于匹配的鲁棒描述符，直接方法直接利用原始测量来优化传感器姿态 [11]，通过最小化基于光度误差或点到平面残差的误差函数，例如，[3, 12, 13, 14]。通过消除耗时的特征提取和匹配，直接方法提供了快速姿态估计。然而，由于没有特征匹配，需要相当准确的状态先验估计以避免局部最小值。

视觉 SLAM 中的直接方法可以广泛地分为密集直接、半密集直接和稀疏直接方法。密集直接方法主要用于具有完整深度测量的 RGB-D 相机，如 [15, 16, 17] 所示，应用图像到模型对齐进行姿态估计。相反，半密集直接方法 [18, 3] 通过利用具有显著灰度梯度的像素来实现直接图像对齐进行估计。稀疏直接方法 [12, 2] 侧重于通过仅使用少数精心选择的原始补丁来提供准确的状态估计，从而与密集和半密集直接方法相比进一步减少了计算负担。

与直接视觉 SLAM 方法不同，直接 LiDAR SLAM 系统 [13, 14, 19, 20] 不区分密集和稀疏方法，并且通常使用每次扫描中的空间下采样或时间下采样的原始点来构建姿态优化的约束。

在我们的工作中，我们利用直接方法的原理来实现 LiDAR 和视觉模块。我们系统的 LiDAR 模块改编自 VoxelMap [14]，而视觉模型基于稀疏直接方法的变体 [12]。虽然我们的视觉模块从 [12] 中的稀疏直接图像对齐中汲取了灵感，但它通过重新利用 LiDAR 点作为视觉地图点而有所不同，从而减轻了密集的后端计算（即特征对齐、滑动窗口优化和/或深度过滤）。

II-B LiDAR-视觉（-惯性）SLAM

在 LiDAR-视觉-惯性 SLAM 中结合多个传感器使系统能够处理各种具有挑战性的环境，尤其是在一个传感器出现故障或部分退化时。受此启发，研究界已经看到了各种 LiDAR-视觉-惯性 SLAM 系统的出现。现有方法通常可以分为两类：松耦合和紧耦合。从两个角度可以确定分类：状态估计级别和原始测量级别。在状态估计级别上，关键是来自一个传感器估计是否作为另一个传感器模型中的优化目标。在原始测量级别上，它涉及是否结合来自不同传感器的原始数据。

张等人提出了一种在状态估计层面松耦合的激光雷达视觉惯性SLAM系统[21]。在此系统中，VIO 子系统仅为 LIO 子系统中的扫描配准提供初始姿态，而不是与扫描配准一起进行联合优化。 VIL-SLAM [22] 采用类似的松耦合方法，不利用 LiDAR、相机和 IMU 测量的联合优化。

一些系统（例如，DEMO [23]，LIMO [24]，CamVox [25]，[26]）使用 3D LiDAR 点为视觉模块提供深度测量 [1, 27, 4]。虽然这些系统表现出测量级紧耦合，但它们在状态估计中仍然保持松耦合，这主要是因为没有直接从 LiDAR 测量中获得的约束在状态估计中。另一个问题是，由于分辨率不匹配，3D LiDAR 点与 2D 图像特征点和/或线没有一一对应关系。这种不匹配需要在深度关联中进行插值，这会引入潜在的错误。为了解决这个问题，DVL-SLAM [28] 采用了一种直接的视觉跟踪方法，其中 LiDAR 点直接投影到图像中以确定对应像素位置的深度。

上述提到的工作还没有在状态估计级别上实现紧耦合。为了追求更高的准确性和鲁棒性，最近涌现了许多联合优化传感器数据以紧密耦合的方式进行的研究。举几个例子，LIC-Fusion [29] 基于 MSCKF [30] 框架紧密融合了 IMU 测量值、稀疏视觉特征以及 LiDAR 平面和边缘特征。随后的 LIC-Fusion2.0 [31] 通过在滑动窗口内实现平面特征跟踪来增强 LiDAR 姿态估计。 VILENS [32] 通过统一的因子图提供视觉、LiDAR 和惯性数据的联合优化，依赖于固定滞后平滑。 R2LIVE [33] 在一个流形上的迭代卡尔曼滤波器 [34] 中紧密融合了 LiDAR、相机和 IMU 测量值。对于 R2LIVE 中的 VIO 子系统，滑动窗口优化用于三角化地图中视觉特征的位置。

一些系统在测量和状态估计级别上都实现了完整的紧耦合。 LVI-SAM [35] 在一个紧密耦合的平滑和映射框架中融合了 LiDAR、视觉和惯性传感器，该框架建立在因子图之上。 VIO 子系统执行视觉特征跟踪并使用 LiDAR 扫描提取特征深度。 R3LIVE [36] 通过 LIO 构建全局地图的几何结构，并通过 VIO 渲染地图纹理。这两个子系统通过将其各自的 LiDAR 或视觉数据与 IMU 融合来共同估计系统状态。它的高级版本 R3LIVE++ [37] 实时估计曝光时间并在预先进行光度校准 [38]，这使得系统能够恢复地图点的辐射度。与大多数先前提到的依赖于基于特征的方法来进行 LIO 和 VIO 子系统的 LiDAR-惯性-视觉系统不同，R3LIVE 系列 [36, 37] 对两者都采用了直接方法，而无需特征提取，使其能够即使在无纹理或无结构的情况下也能捕获细微的环境特征。

我们的系统还使用 LiDAR、图像和 IMU 数据联合估计状态，并在测量级别维护一个紧密耦合的体素图。此外，我们的系统使用直接方法，利用原始 LiDAR 点进行 LiDAR 扫描配准，并使用原始图像块进行视觉跟踪。我们的系统与 R3LIVE（或 R3LIVE++）之间的关键区别在于，R3LIVE（和 R3LIVE++）在 VIO 中以单个像素级别运行，而我们的系统在图像块级别运行。这种差异赋予了我们的系统显著的优势。首先，在稳健性方面，我们的方法使用简化的、一步式的帧到地图稀疏图像对齐进行姿态估计，从而减少了对 R3LIVE 中必须通过帧到帧光流获得的准确初始状态的严重依赖。因此，我们的系统简化并改进了 R3LIVE 中的两阶段帧到帧和帧到地图操作。其次，从计算的角度来看，R3LIVE 中的 VIO 主要采用密集直接方法，这种方法的计算成本很高，需要大量点进行残差构建和渲染。相反，我们的稀疏直接方法提供了更高的计算效率。最后，我们的系统利用原始图像块分辨率的信息，而 R3LIVE 的分辨率被限制在其点图的分辨率。

我们的系统的视觉模块与 DV-LOAM [39]、SDV-LOAM [40] 和 LVIO-Fusion [41] 最为相似，它们将带有图像块的 LiDAR 点投影到新图像中，并通过最小化直接光度误差来跟踪图像。然而，它们存在一些关键差异，例如使用单独的地图进行视觉和 LiDAR，依赖于视觉模块中图像块扭曲中恒定深度的假设，在状态估计级别松散耦合，以及两阶段的帧到帧和帧到关键帧进行图像对齐。相反，我们的系统在迭代卡尔曼滤波器中紧密地整合了帧到地图图像对齐、LiDAR 扫描配准和 IMU 测量。此外，由于 LiDAR 和视觉模块使用单个统一地图，我们的系统可以直接使用 LiDAR 点提供的平面先验来加速图像对齐。

表 I：一些重要的符号

Notations	Explanation
$\displaystyle\boxplus$ / $\displaystyle\boxminus$	The encapsulated “boxplus” and
	“boxminus” operations on the state manifold
$\displaystyle{{}^{G}(\cdot)}$	A vector $\displaystyle{(\cdot)}$ in global world frame
$\displaystyle{{}^{C}(\cdot)}$	A vector $\displaystyle{(\cdot)}$ in camera frame
${}^{I}\mathbf{T}_{L}$	The extrinsic of LiDAR frame w.r.t. IMU frame
${}^{C}\mathbf{T}_{I}$	The extrinsic of IMU frame w.r.t. camera frame
${}^{G}\mathbf{T}_{I}$	The pose of IMU frame at time $\displaystyle k$ w.r.t. the global frame
$\displaystyle\mathbf{x},\widehat{\mathbf{x}}$ , $\displaystyle\bar{\mathbf{x}}$	The ground-truth, predicted and updated estimation of $\displaystyle\mathbf{x}$
$\displaystyle\widehat{\mathbf{x}}^{\kappa}$	The $\displaystyle\kappa$ -th update of $\displaystyle\mathbf{x}$
$\displaystyle\bm{\delta}\mathbf{x}$	The error state between ground-truth $\displaystyle\mathbf{x}$ and its estimation

III 系统概述

我们系统概述如图 1 所示，包含四个部分：ESIKF（第 IV 节）、局部映射（第 V 节）、LiDAR 测量模型（第 VI 节）和视觉测量模型（第 VII 节）。

首先，通过扫描重组将异步采样的 LiDAR 点重新组合成相机采样时间的扫描。然后，我们通过具有顺序状态更新的 ESIKF 将 LiDAR、图像和惯性测量紧密耦合，其中系统状态依次更新，首先由 LiDAR 测量更新，然后由图像测量更新，两者都利用基于单个统一体素地图的直接方法（第 IV 节）。为了在 ESIKF 更新中构建 LiDAR 测量模型（第 VI 节），我们计算了帧到地图的点到平面残差。为了建立视觉测量模型（第 VII 节），我们从地图中提取当前视场内的视觉地图点，利用可见体素查询和按需射线投射；提取后，我们识别并剔除异常视觉地图点（例如，被遮挡的点或深度不连续的点）；然后，我们计算帧到地图的图像光度误差以进行视觉更新。

视觉和 LiDAR 更新的局部地图是一个体素地图结构（第 V 节）：LiDAR 点构建和更新地图的几何结构，而视觉图像将图像块追加到选定的地图点（即视觉地图点）并动态更新参考块。更新的参考块的法向量在单独的线程中进一步细化。

Refer to caption — 图 1： FAST-LIVO2 系统概述。

IV 具有顺序状态更新的误差状态迭代卡尔曼滤波器

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

IV-A 符号和状态转移模型

在我们的系统中，我们假设三个传感器（LiDAR、IMU 和相机）之间的时间偏移是已知的，这可以通过提前校准或同步来实现。我们将 IMU 帧（表示为 $\displaystyle I$ ）作为机体帧，并将第一个机体帧作为全局帧（表示为 $\displaystyle G$ ）。此外，我们假设三个传感器是刚性连接的，并且表 I 中定义的外参是预先校准的。然后，在第 $\displaystyle i$ 个 IMU 测量处的离散状态转移模型为：

\mathbf{x}_{i+1}=\mathbf{x}_{i}\boxplus\left(\Delta t\mathbf{f}\left(\mathbf{x% }_{i},\mathbf{u}_{i},\mathbf{w}_{i}\right)\right)

(1)

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

$\displaystyle\mathcal{M}$	$\displaystyle\triangleq SO(3)\times\mathbb{R}^{16},\ \text{dim}(\mathcal{M})=19$	(2)
$\displaystyle\mathbf{x}$	$\displaystyle\triangleq\begin{bmatrix}{}^{G}\mathbf{R}_{I}^{T}&{}^{G}\mathbf{p% }_{I}^{T}&{}^{G}\mathbf{v}_{I}^{T}&\mathbf{b}_{{g}}^{T}&\mathbf{b}_{{a}}^{T}&{% }^{G}\mathbf{g}^{T}&\tau\end{bmatrix}^{T}\in\mathcal{M}$
$\displaystyle\mathbf{u}$	$\displaystyle\triangleq\begin{bmatrix}\bm{\omega}_{m}^{T}&\mathbf{a}_{m}^{T}% \end{bmatrix}^{T},\hskip 5.69046pt\mathbf{w}\triangleq\begin{bmatrix}\mathbf{n% }_{{g}}^{T}&\mathbf{n}_{{a}}^{T}&\mathbf{n}_{\mathbf{b}_{g}}^{T}&\mathbf{n}_{% \mathbf{b}_{a}}^{T}&{n_{\tau}}\end{bmatrix}^{T}$
$\displaystyle\mathbf{f}$	$\displaystyle(\mathbf{x},\mathbf{u},\mathbf{w})=\begin{bmatrix}\bm{\omega}_{m}% -\mathbf{b}_{{g}}-\mathbf{n}_{{g}}\\ {}^{G}\mathbf{v}_{I}+\frac{1}{2}(^{G}\mathbf{R}_{I}\left(\mathbf{a}_{m}-% \mathbf{b}_{{a}}-\mathbf{n}_{{a}}\right)+{{}^{G}\mathbf{g}})\Delta t\\ {}^{G}\mathbf{R}_{I}\left(\mathbf{a}_{m}-\mathbf{b}_{{a}}-\mathbf{n}_{{a}}% \right)+{{}^{G}\mathbf{g}}\\ \mathbf{n}_{\mathbf{b}_{{g}}}\\ \mathbf{n}_{\mathbf{b}_{{a}}}\\ \mathbf{0}_{3\times 1}\\ n_{\tau}\end{bmatrix}$

其中 ${}^{G}\mathbf{R}_{I}$ ， ${}^{G}\mathbf{p}_{I}$ 和 ${}^{G}\mathbf{v}_{I}$ 分别表示 IMU 在全局坐标系下的姿态、位置和速度， $\displaystyle{{}^{G}\mathbf{g}}$ 是全局坐标系下的重力矢量， $\displaystyle\tau$ 是相对于第一帧的逆相机曝光时间， $\displaystyle n_{\tau}$ 是对 $\displaystyle\tau$ 建模为随机游走的噪声， $\displaystyle\bm{\omega}_{m}$ 和 $\displaystyle\mathbf{a}_{m}$ 是原始 IMU 测量值， $\displaystyle\mathbf{n}_{g}$ 和 $\displaystyle\mathbf{n}_{a}$ 是 $\displaystyle\bm{\omega}_{m}$ 和 $\displaystyle\mathbf{a}_{m}$ 中的测量噪声， $\displaystyle\mathbf{b}_{a}$ 和 $\displaystyle\mathbf{b}_{g}$ 是 IMU 偏差，它们被建模为由高斯噪声 $\displaystyle\mathbf{n}_{\mathbf{b}_{g}}$ 和 $\displaystyle\mathbf{n}_{\mathbf{b}_{a}}$ 驱动的随机游走。

IV-B 扫描重组

我们采用扫描重组，将高频、顺序采样的 LiDAR 原始点分割成相机采样时刻的不同 LiDAR 扫描，如图 2 所示。这确保了相机和 LiDAR 数据以相同频率（例如，10 Hz）同步，从而允许在同一时间更新状态。

IV-C 传播

在 ESIKF 框架中，状态和协方差从时间 $\displaystyle t_{k-1}$ （接收最后一个 LiDAR 扫描和图像帧时）传播到时间 $\displaystyle t_{k}$ （接收当前 LiDAR 扫描和图像帧时）。此前向传播预测了 $\displaystyle t_{k-1}$ 和 $\displaystyle t_{k}$ 期间每个 IMU 输入 $\displaystyle\mathbf{u}_{i}$ 的状态，方法是将 (1) 中的过程噪声 $\displaystyle\mathbf{w}_{i}$ 设置为零。将传播的状态表示为 $\displaystyle\widehat{\mathbf{x}}$ ，协方差表示为 $\displaystyle\widehat{\mathbf{P}}$ ，它们将作为第 IV-D 节中后续更新的先验分布。此外，为了补偿运动失真，我们按照 [42] 进行反向传播，确保 LiDAR 扫描中的点在扫描结束时间 $\displaystyle t_{k}$ 被“测量”。请注意，为了简化符号，我们省略了所有状态向量中的下标 $\displaystyle k$ 。

1 Scan recombination to synchronize LiDAR and image data at the camera rate;

2 Forward propagation to obtain state prediction

\displaystyle\widehat{\mathbf{x}}

and its covariance

\displaystyle\widehat{\mathbf{P}}

;

3 Backward propagation for LiDAR points motion compensation;

// Point-to-plane LiDAR update

\displaystyle\kappa=-1

\displaystyle\widehat{\mathbf{x}}^{\kappa=0}=\widehat{\mathbf{x}}

;

5 repeat

\displaystyle\kappa=\kappa+1

;

7 Compute residual

\displaystyle\mathbf{z}_{l}^{\kappa}

and Jacobin

\displaystyle\mathbf{H}_{l}^{\kappa}

;

8 Compute the state update

\displaystyle\widehat{\mathbf{x}}^{\kappa+1}

;

9until $\displaystyle\|\widehat{\mathbf{x}}^{\kappa+1}\boxminus\widehat{\mathbf{x}}^{% \kappa}\|<\epsilon$ ;

\displaystyle\widehat{\mathbf{x}}=\widehat{\mathbf{x}}^{\kappa+1}

\displaystyle\widehat{\mathbf{P}}=\left(\mathbf{I}-\mathbf{K}\mathbf{H}\right)% {\widehat{\mathbf{P}}}

;

// Sparse direct visual update

\displaystyle level=-1

;

12 repeat

\displaystyle\kappa=-1

\displaystyle\widehat{\mathbf{x}}^{\kappa=0}=\widehat{\mathbf{x}}

;

\displaystyle level=level+1

;

15 repeat

\displaystyle\kappa=\kappa+1

;

17 Compute residual

\displaystyle\mathbf{z}_{c}^{\kappa}

and Jacobin

\displaystyle\mathbf{H}_{c}^{\kappa}

;

18 Compute the state update

\displaystyle\widehat{\mathbf{x}}^{\kappa+1}

;

19 until $\displaystyle\|\widehat{\mathbf{x}}^{\kappa+1}\boxminus\widehat{\mathbf{x}}^{% \kappa}\|<\epsilon$ ;

\displaystyle\widehat{\mathbf{x}}=\widehat{\mathbf{x}}^{\kappa+1}

;

22until $\displaystyle level>=2$ ;

\displaystyle\bar{\mathbf{x}}=\widehat{\mathbf{x}}^{\kappa+1}

;

\displaystyle\bar{\mathbf{P}}=\left(\mathbf{I}-\mathbf{K}\mathbf{H}\right){% \widehat{\mathbf{P}}}

Algorithm 1 Sequential State Update

IV-D 顺序更新

IMU 传播的状态 $\displaystyle\widehat{\mathbf{x}}$ 和协方差 $\displaystyle\widehat{\mathbf{P}}$ 为 $\displaystyle\mathbf{x}$ （系统在时间 $\displaystyle t_{k}$ 的状态）施加了一个先验分布，如下所示：

\mathbf{x}\boxminus\widehat{\mathbf{x}}\sim\mathcal{N}(\mathbf{0},\widehat{% \mathbf{P}})

(3)

我们将上述先验分布表示为 $\displaystyle p(\mathbf{x})$ ，并将 LiDAR 和相机的测量模型表示为：

\begin{split}~{}\begin{bmatrix}\mathbf{y}_{l}\\ \mathbf{y}_{c}\end{bmatrix}=\begin{bmatrix}\mathbf{h}_{l}(\mathbf{x},\mathbf{v% }_{l})\\ \mathbf{h}_{c}(\mathbf{x},\mathbf{v}_{c})\end{bmatrix}\end{split}

(4)

其中 $\displaystyle\mathbf{v}_{l}\sim\mathcal{N}(\mathbf{0},\bm{\Sigma}_{\mathbf{v}_% {l}})$ 和 $\displaystyle\mathbf{v}_{c}\sim\mathcal{N}(\mathbf{0},\bm{\Sigma}_{\mathbf{v}_% {c}})$ 分别表示 LiDAR 和相机的测量噪声。

标准 ESIKF [43] 将使用所有当前测量值（包括 LiDAR 测量值 $\displaystyle\mathbf{y}_{l}$ 和图像测量值 $\displaystyle\mathbf{y}_{c}$ ）来更新状态 $\displaystyle\mathbf{x}$ 。但是，LiDAR 和图像测量值是两种不同的传感模式，其数据维数不匹配。此外，图像测量的融合可以在图像金字塔的不同级别进行。为了解决维数不匹配并为每个模块提供更大的灵活性，我们提出了一种顺序更新策略。这种策略在理论上等同于使用所有测量值的标准更新，假设给定状态向量 $\displaystyle\mathbf{x}$ （即，受统计独立噪声影响的测量值）时，LiDAR 测量值 $\displaystyle\mathbf{y}_{l}$ 和图像测量值 $\displaystyle\mathbf{y}_{c}$ 统计独立。

为了引入顺序更新，我们将当前状态 $\displaystyle\mathbf{x}$ 的总条件分布重写为：

\displaystyle\displaystyle\begin{aligned} p(\mathbf{x}\!\mid\!\mathbf{y}_{l},% \mathbf{y}_{c})&\propto p(\mathbf{x},\mathbf{y}_{l},\mathbf{y}_{c})=p(\mathbf{% y}_{c}\!\mid\!\mathbf{x},\mathbf{y}_{l})p(\mathbf{x},\mathbf{y}_{l})\\ &=p(\mathbf{y}_{c}\!\mid\!\mathbf{x})\underbrace{p(\mathbf{y}_{l}\!\mid\!% \mathbf{x})p(\mathbf{x})}_{\propto p(\mathbf{x}\mid\mathbf{y}_{l})}\end{aligned}

(5)

等式 (5) 意味着总条件分布 $\displaystyle p(\mathbf{x}\mid\mathbf{y}_{l},\mathbf{y}_{c})$ 可以通过两个顺序贝叶斯更新获得。第一步只将 LiDAR 测量值 $\displaystyle\mathbf{y}_{l}$ 与 IMU 传播的先验分布 $\displaystyle p(\mathbf{x})$ 融合，以获得分布 $\displaystyle p(\mathbf{x}\!\mid\!\mathbf{y}_{l})$ ：

p(\mathbf{x}\!\mid\!\mathbf{y}_{l})\propto p(\mathbf{y}_{l}\!\mid\!\mathbf{x})% p(\mathbf{x})

(6)

然后，第二步将相机测量值 $\displaystyle\mathbf{y}_{c}$ 与 $\displaystyle p(\mathbf{x}\!\mid\!\mathbf{y}_{l})$ 融合，以获得 $\displaystyle\mathbf{x}$ 的最终后验分布：

p(\mathbf{x}\!\mid\!\mathbf{y}_{l},\mathbf{y}_{c})\propto p(\mathbf{y}_{c}\!% \mid\!\mathbf{x})p(\mathbf{x}\!\mid\!\mathbf{y}_{l})

(7)

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

q(\mathbf{x}\!\mid\!\mathbf{y})\propto q(\mathbf{y}\!\mid\!\mathbf{x})q(% \mathbf{x})

(8)

为了对 LiDAR 或图像测量值进行 (8) 中的融合，我们将先验分布 $\displaystyle q(\mathbf{x})$ 和测量模型 $\displaystyle q(\mathbf{y}\!\mid\!\mathbf{x})$ 详细说明如下。对于先验分布 $\displaystyle q(\mathbf{x})$ ，用 $\displaystyle\mathbf{x}=\widehat{\mathbf{x}}\boxplus\bm{\delta}\mathbf{x}$ 表示，其中 $\displaystyle\bm{\delta}\mathbf{x}\sim\mathcal{N}(\mathbf{0},\widehat{\mathbf{% P}})$ 。在 LiDAR 更新的情况下（即第一步）， $\displaystyle(\widehat{\mathbf{x}},\widehat{\mathbf{P}})$ 是从传播步骤获得的状态和协方差。在视觉更新的情况下（即第二步）， $\displaystyle(\widehat{\mathbf{x}},\widehat{\mathbf{P}})$ 是从 LiDAR 更新获得的收敛状态和协方差。

为了获得测量模型分布 $\displaystyle q(\mathbf{y}\!\mid\!\mathbf{x})$ ，用 $\displaystyle\kappa$ 表示第 $\displaystyle\widehat{\mathbf{x}}^{\kappa}$ 次迭代估计的状态，其中 $\displaystyle\widehat{\mathbf{x}}^{0}=\widehat{\mathbf{x}}$ 。通过在 $\displaystyle\widehat{\mathbf{x}}^{\kappa}$ 处进行一阶泰勒展开，对测量模型 (4)（无论是 LiDAR 还是相机测量）进行近似，得到：

		$\displaystyle\displaystyle\mathbf{y}\!\mid\!\mathbf{x}\simeq\underbrace{% \mathbf{h}(\widehat{\mathbf{x}}^{\kappa},\mathbf{0})}_{{\mathbf{z}^{\kappa}}}+% \mathbf{H}^{\kappa}\bm{\delta}\mathbf{x}^{\kappa}+\mathbf{L}^{\kappa}\,\mathbf% {v}$		(9)
		$\displaystyle\displaystyle q(\mathbf{y}\!\mid\!\mathbf{x})\simeq\mathcal{N}(% \mathbf{h}(\widehat{\mathbf{x}}^{\kappa},\mathbf{0})+\mathbf{H}^{\kappa}\bm{% \delta}\mathbf{x}^{\kappa},\mathbf{R})$		(10)

其中 $\displaystyle\bm{\delta}\mathbf{x}^{\kappa}=\mathbf{x}\boxminus\widehat{% \mathbf{x}}^{\kappa}$ ， $\displaystyle\mathbf{z}^{\kappa}$ 是残差， $\displaystyle\mathbf{L}^{\kappa}\,\mathbf{v}\sim\mathcal{N}(\mathbf{0},\mathbf% {R})$ 是集中测量噪声， $\displaystyle\mathbf{H}^{\kappa}$ 和 $\displaystyle\mathbf{L}^{\kappa}$ 分别是 $\displaystyle\mathbf{h}(\widehat{\mathbf{x}}^{\kappa}\boxplus\bm{\delta}% \mathbf{x}^{\kappa},\mathbf{v})$ 相对于 $\displaystyle\bm{\delta}\mathbf{x}^{\kappa}$ 和 $\displaystyle\mathbf{v}$ 的雅可比矩阵，在零点处求值。

然后，将先验分布 $\displaystyle q(\mathbf{x})$ 和测量分布 $\displaystyle q(\mathbf{y}\!\mid\!\mathbf{x})$ 代入 (10) 中的后验分布 (8)，并执行最大似然估计 (MLE)，我们可以从 ESIKF 框架中的标准更新步骤 [43] 中获得 $\displaystyle\bm{\delta}\mathbf{x}^{\kappa}$ （因此 $\displaystyle\mathbf{x}^{\kappa}$ ）的最大后验估计 (MAP)：

\begin{split}~{}\mathbf{K}&=\left((\mathbf{H}^{\kappa})^{T}{\mathbf{R}}^{-1}% \mathbf{H}^{\kappa}+{\widehat{\mathbf{P}}}^{-1}\right)^{-1}(\mathbf{H}^{\kappa% })^{T}\mathbf{R}^{-1},\\ \widehat{\mathbf{x}}^{\kappa+1}&=\widehat{\mathbf{x}}^{\kappa}\!\boxplus\!% \left(-\mathbf{K}{\mathbf{z}}^{\kappa}-(\mathbf{I}-\mathbf{K}\mathbf{H}^{% \kappa})\left(\widehat{\mathbf{x}}^{\kappa}\!\boxminus\widehat{\mathbf{x}}% \right)\right)\end{split}

(11)

收敛的状态和协方差矩阵然后构成后验分布 $\displaystyle q(\mathbf{x}|\mathbf{y})$ 的均值和协方差。

文献中已经研究了具有顺序更新的卡尔曼滤波器，例如在 [44, 45] 中。本文将这种方法应用于 LiDAR 和相机系统的 ESIKF。具有顺序更新的 ESIKF 的实现细节见算法 1。在第一步（第 6-10 行），误差状态从 LiDAR 测量值（第 VI-A 节）迭代更新，直到收敛。收敛的状态和协方差估计，再次用 $\displaystyle\widehat{\mathbf{x}}$ 和 $\displaystyle\widehat{\mathbf{P}}$ 表示，用于更新地图的几何结构（第 V-B 节），并在第二步视觉更新（第 13-23 行）中对图像金字塔的每个级别（第 VII-B 节）进行细化，直到收敛。最佳状态和协方差，用 $\displaystyle\bar{\mathbf{x}}$ 和 $\displaystyle\bar{\mathbf{P}}$ 表示，用于传播传入的 IMU 测量值（第 IV-C 节）并更新地图的视觉结构（第 V-D 节和 V-E 节）。

V 局部映射

V-A 地图结构

我们的地图采用了一种自适应体素结构，如 [14] 中所述，该结构由一个哈希表和每个哈希条目（图 1）的八叉树组织。哈希表管理根体素，每个根体素具有 $\displaystyle 0.5\times 0.5\times 0.5$ 米的固定尺寸。每个根体素封装一个八叉树结构，以进一步组织不同尺寸的叶体素。叶体素代表一个局部平面，并存储一个平面特征（即平面中心、法向量和不确定性）以及位于该平面上的 LiDAR 原始点集。其中一些点附带三级图像块（ $\displaystyle 8\times 8$ 块大小），我们将其称为视觉地图点。收敛的视觉地图点只附带参考块，而未收敛的视觉地图点则附带参考块和其他可见块（见第 V-E 节）。叶体素的不同尺寸使它能够表示不同尺度的局部平面，从而适应不同结构的环境 [14]。

为了防止地图尺寸不受限制，我们只保留 LiDAR 当前位置周围长度为 $\displaystyle L$ 的较大局部区域内的局部地图，如图 3 的二维示例所示。最初，地图是一个以 LiDAR 起始位置 $\displaystyle\mathbf{p}_{0}$ 为中心的立方体。 LiDAR 的探测区域被可视化为一个以其当前位置为中心，半径由 LiDAR 探测范围定义的球体。当 LiDAR 移动到一个新的位置 $\displaystyle\mathbf{p}_{1}$ ，其中探测区域触及地图边界时，我们通过距离 $\displaystyle d$ 将地图从边界移开。随着地图的移动，包含移出局部地图区域的内存将被重置，以存储移入局部地图的新区域。这种环形缓冲器方法确保我们的局部地图在固定大小的内存中得到维护。环形缓冲器哈希地图的实现细节见 [46]。地图移动检查在每个 ESIKF 更新步骤之后执行。

V-B 几何构造和更新

地图的几何形状从 LiDAR 点测量中构建和更新。具体来说，在 ESIKF 中更新 LiDAR 后（第 IV 节），我们将 LiDAR 扫描中的所有点注册到全局帧。对于每个注册的 LiDAR 点，我们确定其在 Hash 地图中的定位根体素。如果体素不存在，我们用新点初始化体素并将其索引到 Hash 地图中。如果确定的体素已经在地图中存在，我们将该点附加到现有的体素。在一次扫描中的所有点都被分配后，我们进行几何构造和更新，如下所示。

对于新创建的体素，我们根据奇异值分解确定所有包含的点是否位于一个平面上。如果是，我们计算中心点 $\displaystyle\mathbf{q}=\bar{\mathbf{p}}$ 、平面法线 $\displaystyle\mathbf{n}$ 和 $\displaystyle(\mathbf{q},\mathbf{n})$ 的协方差矩阵，表示为 $\displaystyle\bm{\Sigma}_{\mathbf{n,\mathbf{q}}}$ 。 $\displaystyle\bm{\Sigma}_{\mathbf{n,\mathbf{q}}}$ 用于表征平面不确定性，它来自于姿态估计不确定性和点测量噪声。平面标准的详细说明，以及平面参数和不确定性的计算，请参考我们之前的工作[14]。如果包含的点不在一个平面上，则该体素将被连续细分为八个更小的八分体，直到确定子体素中的点形成一个平面，或达到最大层数（例如，3）。在后一种情况下，叶体素中的点将被丢弃。因此，该地图仅包含被识别为平面的体素（根体素或子体素）。

对于添加了新点的现有体素，我们评估新点是否仍与根体素或子体素中现有点形成平面。如果没有，我们将如上进行体素细分。如果是，我们也会更新平面的参数 ( $\displaystyle\mathbf{q}$ , $\displaystyle\mathbf{n}$ ) 和协方差 $\displaystyle\bm{\Sigma}_{\mathbf{n,\mathbf{q}}}$ 。一旦平面参数收敛（参见 [14]），该平面将被视为成熟，并且该平面上新的点将被丢弃。此外，成熟的平面将固定其估计的平面参数 ( $\displaystyle\mathbf{q}$ , $\displaystyle\mathbf{n}$ ) 和协方差 $\displaystyle\bm{\Sigma}_{\mathbf{n,\mathbf{q}}}$ 。

平面上（在根体素或子体素中）的 LiDAR 点将用于在后续部分生成视觉地图点。对于成熟的平面，最新的 50 个 LiDAR 点是视觉地图点生成的候选点，而对于未成熟的平面，所有 LiDAR 点都是候选点。视觉地图点生成过程将识别这些候选点中的一些点作为视觉地图点，并为它们附加图像补丁以进行图像对齐。

V-C 视觉地图点生成和更新

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

V-D 参考补丁更新

由于添加了新的补丁，视觉地图点可能具有多个补丁。我们需要为视觉更新中的图像对齐选择一个参考补丁。具体来说，我们根据光度相似性和视角对每个补丁 $\displaystyle\mathbf{f}$ 进行评分，如下所示：

$\displaystyle\displaystyle\text{NCC}(\mathbf{f},\mathbf{g})$	$\displaystyle\displaystyle=\frac{\sum_{x,y}[\mathbf{f}(x,y)-\bar{\mathbf{f}}][% \mathbf{g}(x,y)-\bar{\mathbf{g}}]}{\sqrt{\sum_{x,y}[\mathbf{f}(x,y)-\bar{% \mathbf{f}}]^{2}\sum_{x,y}[\mathbf{g}(x,y)-\bar{\mathbf{g}}]^{2}}}$
$\displaystyle\displaystyle c$	$\displaystyle\displaystyle=\frac{\mathbf{n}\cdot{\mathbf{p}}}{\left\\|\mathbf{p% }\right\\|},\,\,\,\,\omega_{1}=\frac{1}{1+e^{\text{tr}(\bm{\Sigma}_{\mathbf{n}}% )}}$	(12)
$\displaystyle\displaystyle S$	$\displaystyle\displaystyle=(1-\omega_{1})\cdot{\frac{1}{n}\sum_{i=1}^{n}\text{% NCC}(\mathbf{f},\mathbf{g}_{i})}+\omega_{1}\cdot{c}$

其中 $\displaystyle\text{NCC}(\mathbf{f},\mathbf{g})$ 表示用于测量补丁 $\displaystyle\mathbf{f}$ 和 $\displaystyle\mathbf{g}$ 在两个补丁的第 $\displaystyle 0$ 个金字塔级别（具有最高分辨率的级别）上的相似性的归一化互相关 (NCC)，对两个补丁都应用了均值减法， $\displaystyle c$ 表示法向量 $\displaystyle\mathbf{n}$ 和待评估补丁 $\displaystyle\mathbf{f}$ 的视角 $\displaystyle\mathbf{p}/\|\mathbf{p}\|$ 之间的余弦相似度。当补丁直接面对地图点所在的平面时， $\displaystyle c$ 的值为 1。总体得分 $\displaystyle S$ 是通过对加权 NCC 和 $\displaystyle c$ 求和来计算的，其中前者表示待评估的补丁 $\displaystyle\mathbf{f}$ 与所有其他补丁 $\displaystyle\mathbf{g}_{i}$ 之间的平均相似度，而 $\displaystyle\text{tr}(\bm{\Sigma}_{\mathbf{n}})$ 表示法向量的协方差矩阵的迹。

在附加到视觉地图点的所有补丁中，得分最高的补丁被更新为参考补丁。上述评分机制倾向于选择其（1）外观（在 NCC 方面）与大多数其他补丁相似的参考补丁，这是一种由 MVS [47] 使用的技术，用于避免动态物体上的补丁；（2）视角垂直于平面，从而以高分辨率保持纹理细节。相反，我们之前工作 FAST-LIVO [8] 和先前技术 [4] 中的参考补丁更新策略直接选择与当前帧视图方向差异最小的补丁，导致所选参考补丁非常接近当前帧，因此对当前姿态更新施加了较弱的约束。

V-E 法线细化

假设每个视觉地图点位于一个小局部平面上。现有工作 [4, 2, 8] 假设补丁中的所有像素都具有相同的深度，这是一个在一般情况下不成立的错误假设。我们使用从 LiDAR 点计算出的平面参数（如第 V-B 节所述）以实现更高的精度。此平面法线对于在视觉更新过程中执行用于图像对齐的仿射扭曲至关重要。为了进一步提高仿射扭曲的精度，可以从附加到视觉地图点的补丁中进一步细化平面法线。具体而言，我们通过最小化参考补丁中与附加到视觉地图点的其他补丁之间的光度误差来细化平面法线。

V-E1 仿射扭曲

仿射扭曲用于将补丁像素从参考帧（即源补丁）变换到其余帧中的补丁像素（即目标补丁），如图 4(a) 所示。令 $\displaystyle\mathbf{u}^{j}_{r}$ 为源补丁中的第 $\displaystyle j$ 个像素坐标， $\displaystyle\mathbf{u}^{j}_{i}$ 为第 $\displaystyle j$ 个目标补丁中的第 $\displaystyle i$ 个像素坐标。假设补丁中的所有像素都位于具有法线 $\displaystyle{{}^{I_{r}}\mathbf{n}}$ 和视觉地图点位置 ${}^{I_{r}}{\mathbf{p}}$ 的局部平面上（对应于源和目标补丁的中心像素），两者都在源补丁框架中表示，我们有：

	$\displaystyle\mathbf{u}^{j}_{i}$	$\displaystyle=\mathbf{A}^{i}_{r}\mathbf{u}^{j}_{r}$		(13)
	$\displaystyle\mathbf{A}^{i}_{r}$	$\displaystyle=\mathbf{P}({{}^{I_{i}}\mathbf{R}}_{I_{r}}+{{}^{I_{i}}\mathbf{t}}% _{I_{r}}\frac{1}{{{}^{I_{r}}\mathbf{n}^{T}}\cdot{{}^{I_{r}}{\mathbf{p}}}}{{}^{% I_{r}}\mathbf{n}^{T}})\mathbf{P}^{-1}$		(13)

其中 $\displaystyle\mathbf{A}_{r}^{i}$ 表示将像素坐标从源（或参考）补丁变换到第 $\displaystyle i$ 个目标补丁的仿射扭曲矩阵， $\displaystyle{{}^{I_{i}}\mathbf{R}}_{I_{r}}$ 和 $\displaystyle{{}^{I_{i}}\mathbf{t}}_{I_{r}}$ 表示参考帧 $\displaystyle I_{r}$ 相对于目标帧 $\displaystyle I_{i}$ 的相对姿态。为了直接使用鱼眼图像，而不将其校正为针孔图像，我们根据不同的相机模型实现投影矩阵 $\displaystyle\mathbf{P}$ 和反投影矩阵 $\displaystyle\mathbf{P}^{-1}$ （例如， $\displaystyle\mathbf{P}$ 是针孔相机模型的相机内参矩阵）。

V-E2 法线优化

为了细化平面法线 ${}^{I_{r}}\mathbf{n}$ ，我们最小化参考补丁与第 $\displaystyle 0$ 个金字塔级别（即最高分辨率级别）上的其他图像补丁之间的光度误差：

\displaystyle\displaystyle^{I_{r}}\mathbf{n}^{*}

\displaystyle\displaystyle=\arg\min_{{}^{I_{r}}\mathbf{n}\in\mathbb{S}^{2}}% \sum_{i\in S}\sum_{j=1}^{N^{2}}\left\|\tau_{i}\mathbf{I}_{i}(\mathbf{A}^{i}_{r% }\mathbf{u}^{j}_{r})-\tau_{r}\mathbf{I}_{r}(\mathbf{u}^{j}_{r})\right\|_{2}

(14)

其中 $\displaystyle N$ 是补丁大小， $\displaystyle\tau_{r}$ 和 $\displaystyle\tau_{i}$ 分别是参考帧和第 $\displaystyle i$ 个目标帧的倒数曝光时间。 $\displaystyle\mathbf{I}_{r}(\mathbf{u}^{j}_{r})$ 表示参考帧中的第 $\displaystyle j$ 个补丁像素， $\displaystyle\mathbf{I}_{i}(\mathbf{A}^{i}_{r}\mathbf{u}^{j}_{r})$ 表示第 $\displaystyle i$ 个目标帧中的第 $\displaystyle j$ 个补丁像素， $\displaystyle S$ 是所有目标帧的集合。

V-E3 优化变量变换

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

	$\displaystyle\mathbf{M}$	$\displaystyle=\begin{bmatrix}\mathbf{M}_{x}\\ \mathbf{M}_{y}\\ \frac{1}{{}^{I_{r}}\mathbf{p}_{z}}-\frac{{}^{I_{r}}\mathbf{p}_{x}}{{}^{I_{r}}% \mathbf{p}_{z}}\mathbf{M}_{x}-\frac{{}^{I_{r}}\mathbf{p}_{y}}{{}^{I_{r}}% \mathbf{p}_{z}}\mathbf{M}_{y}\end{bmatrix}=\mathbf{B}\mathbf{m}+\mathbf{b}$		(15)
	$\displaystyle\mathbf{B}$	$\displaystyle=\begin{bmatrix}1&0\\ 0&1\\ -\frac{{{}^{I_{r}}\mathbf{p}}_{x}}{{{}^{I_{r}}\mathbf{p}}_{z}}&-\frac{{{}^{I_{% r}}\mathbf{p}}_{y}}{{{}^{I_{r}}\mathbf{p}}_{z}}\end{bmatrix},\mathbf{b}=\begin% {bmatrix}0\\ 0\\ \frac{1}{{{}^{I_{r}}\mathbf{p}}_{z}}\end{bmatrix},\mathbf{m}=\begin{bmatrix}% \mathbf{M}_{x}\\ \mathbf{M}_{y}\end{bmatrix}\in\mathbb{R}^{2}$		(15)

其中 $\displaystyle{{}^{I_{r}}\mathbf{p}}_{z}\neq 0$ ，因为无法为视觉地图点选择这样的参考补丁。 ${}^{I_{r}}\mathbf{n}$ 、 $\displaystyle\mathbf{M}$ 和 $\displaystyle\mathbf{m}$ 之间的关系如图 4 (b) 所示。

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

{}^{I_{r}}\mathbf{n}^{*}=\frac{\mathbf{M}^{*}}{\|\mathbf{M}^{*}\|},\mathbf{M}^% {*}=\mathbf{B}\mathbf{m}^{*}+\mathbf{b}

(16)

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

VI LiDAR 测量模型

本节详细介绍了第 IV-D 节中 ESIKF 的 LiDAR 更新中使用的 LiDAR 测量模型 $\displaystyle\mathbf{y}_{l}=\mathbf{h}_{l}(\mathbf{x},\mathbf{v}_{l})$ 。

VI-A 点到平面 LiDAR 测量模型

在获得扫描中的无失真点 $\displaystyle\{{}^{L}\mathbf{p}_{j}\}$ 之后，我们使用在 LiDAR 更新的第 $\displaystyle\kappa$ 次迭代中估计的状态 $\displaystyle\widehat{\mathbf{x}}^{\kappa}$ 将它们投影到全局坐标系中：

^{G}{\widehat{\mathbf{p}}^{\kappa}_{j}}={}^{G}{\widehat{\mathbf{T}}}^{\kappa}_% {I}{}^{I}{\mathbf{T}}_{L}{}^{L}{\mathbf{p}}_{j}

(17)

然后，我们识别 ${}^{G}{\widehat{\mathbf{p}}^{\kappa}_{j}}$ 所在的 Hash 地图中的根或子体素。如果没有找到体素或体素不包含平面，则丢弃该点。否则，我们使用体素中的平面为 LiDAR 点建立测量方程。具体来说，我们假设给定精确的 LiDAR 姿态 ${}^{G}\mathbf{T}_{I}$ ，真实的 LiDAR 点 ${}^{L}{\mathbf{p}^{gt}_{j}}$ 应该位于体素中法向量为 $\displaystyle\mathbf{n}^{gt}_{j}$ 、中心点为 $\displaystyle{{\mathbf{q}}^{gt}_{j}}$ 的平面上。即，

\displaystyle\displaystyle\mathbf{0}

\displaystyle\displaystyle=({\mathbf{n}_{j}^{gt}})^{T}({}^{G}{\mathbf{T}}_{I}{% }^{I}{\mathbf{T}}_{L}\,^{L}{\mathbf{p}}_{j}^{gt}-\mathbf{q}_{j}^{gt})

(18)

由于地面真值点 ${}^{L}{\mathbf{p}^{gt}_{j}}$ 被测量为 ${}^{L}{\mathbf{p}_{j}}$ ，带有测距和方位噪声 $\displaystyle\bm{\delta}{{}^{L}{\mathbf{p}_{j}}}$ ，因此我们有 ${}^{L}{\mathbf{p}^{gt}_{j}}={}^{L}\mathbf{p}_{j}-\bm{\delta}{{}^{L}{\mathbf{p}% _{j}}}$ 。同样，平面参数 $\displaystyle(\mathbf{n}^{gt}_{j},{\mathbf{q}}^{gt}_{j})$ 被估计为 $\displaystyle(\mathbf{n}_{j},{\mathbf{q}}_{j})$ ，协方差为 $\displaystyle\bm{\Sigma}_{\mathbf{n,\mathbf{q}}}$ （第 V-B 节），因此我们有： $\displaystyle\mathbf{n}_{j}^{gt}=\mathbf{n}_{j}\boxminus\bm{\delta}\mathbf{n}_% {j},\,\,\,{\mathbf{q}}_{j}^{gt}={\mathbf{q}}_{j}-\bm{\delta}{\mathbf{q}_{j}}$ 。因此，

\displaystyle\displaystyle\underbrace{\mathbf{0}}_{\mathbf{y}_{l}}

\displaystyle\displaystyle=\underbrace{{(\mathbf{n}_{j}\boxminus\bm{\delta}% \mathbf{n}_{j})}^{T}({}^{G}{\mathbf{T}}_{I}{}^{I}{\mathbf{T}}_{L}({}^{L}% \mathbf{p}_{j}-\bm{\delta}{{}^{L}{\mathbf{p}_{j}}})-({\mathbf{q}}_{j}-\bm{% \delta}{\mathbf{q}_{j}}))}_{\mathbf{h}_{l}(\mathbf{x},\mathbf{v}_{l})}

(19)

其中测量噪声 $\displaystyle\mathbf{v}_{l}=(\bm{\delta}{{}^{L}{\mathbf{p}_{j}}},\bm{\delta}% \mathbf{n}_{j},\bm{\delta}{\mathbf{q}_{j}})$ 分别由与 LiDAR 点、法向量和平面中心相关的噪声组成。

VI-B 具有光束发散的 LiDAR 测量噪声

在 [14] 中，LiDAR 点 $\displaystyle\bm{\delta}{{}^{L}{\mathbf{p}_{j}}}$ 在本地 LiDAR 坐标系中的不确定性被分解为两个组成部分，即由激光飞行时间 (TOF) 引起的测距不确定性 $\displaystyle\delta d$ 和由编码器产生的方位方向不确定性 $\displaystyle\bm{\delta}\bm{\omega}$ 。除了这些不确定性之外，我们还考虑由激光束发散角 $\displaystyle\theta$ 引起的不确定性，如图 5 所示。随着方位方向和法向量之间的角度 $\displaystyle\varphi$ 增加，LiDAR 点的测距不确定性显着增加，而方位方向不确定性保持不变。由激光束发散角引起的 $\displaystyle\delta d$ 可以建模为：

\delta d=L_{2}-L_{1}=d\!\left(\frac{\cos\varphi}{\cos(\theta+\varphi)}-\frac{% \cos\varphi}{\cos(\theta-\varphi)}\right)

(20)

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

VII 视觉测量模型

本节详细介绍了 ESIKF 视觉更新中使用的视觉测量模型 $\displaystyle\mathbf{y}_{c}=\mathbf{h}_{c}(\mathbf{x},\mathbf{v}_{c})$ ，在第 IV-D 节中。

VII-A 视觉地图点选择

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

VII-A1 可见体素查询

由于地图中体素数量巨大，识别当前帧 FoV 内的地图体素具有挑战性。为了解决这个问题，我们轮询当前扫描中 LiDAR 点击中的体素。这可以通过使用测量的点位置查询体素哈希表来有效地完成。如果相机 FoV 与 LiDAR FoV 大致重叠，则相机 FoV 中的地图点也可能位于这些体素中。我们还轮询由地图点击中的体素，这些地图点在先前图像帧中被识别为可见（通过相同的体素查询和光线投射），假设两个连续图像帧具有较大的 FoV 重叠。最后，可以通过对这两种体素中包含的地图点进行视场检查来获得当前的视觉子地图。

VII-A2 按需光线投射

在大多数情况下，可以通过上述体素查询获得视觉子地图。但是，当 LiDAR 传感器距离物体太近时（称为近距离盲区），它可能无法返回任何点。此外，摄像头的视场可能无法完全覆盖 LiDAR 的视场。为了在这些情况下召回更多视觉地图点，我们采用了如图 6 所示的光线投射策略。我们将图像划分为均匀的网格单元，每个单元包含 $\displaystyle 30\times 30$ 个像素，并将从体素查询获得的视觉地图点投影到网格单元上。对于每个没有被这些视觉地图点占据的图像网格单元，沿着中心像素向后投射一条光线，其中样本点在深度方向上从 $\displaystyle d_{\min}$ 到 $\displaystyle d_{\max}$ 均匀分布在光线上。为了减少计算量，样本点在相机机体坐标系中的位置是预先计算的。对于每个采样点，我们评估对应体素的状态：如果体素包含在投影后位于该网格单元中的地图点，我们将这些地图点合并到视觉子地图中，并停止该光线的处理。否则，我们将继续到光线上的下一个采样点，直到达到最大深度 $\displaystyle d_{\max}$ 。在通过光线投射处理完所有未被占据的图像网格单元后，我们将获得一组分布在整个图像中的视觉地图点。

VII-A3 剔除异常值

在体素查询和光线投射之后，我们获得了当前帧 FoV 中的所有视觉地图点。然而，这些视觉地图点可能在当前帧中被遮挡，具有不连续的深度，其参考补丁是在较大的视角下拍摄的，或者在当前帧中具有较大的视角，所有这些都会严重降低图像对齐精度。为了解决第一个问题，我们将子地图中的所有视觉地图点投影到当前帧中，使用 LiDAR 更新后的姿态，并在每个 $\displaystyle 30\times 30$ 像素的网格单元中保留最低深度的点。为了解决第二个问题，我们将当前 LiDAR 扫描中的 LiDAR 点投影到当前帧，生成深度图。通过比较视觉地图点的深度与其在深度图中的 $\displaystyle 9\times 9$ 邻居，我们确定它们的遮挡和深度变化。被遮挡和深度不连续的地图点被剔除（见图 7）。为了解决第三和第四个问题，我们删除了参考补丁或当前补丁的视角（即，法向量和从视觉地图点到补丁光学中心的指向之间的角度）过大（例如，超过 80°）的点。剩余的视觉地图点将用于对齐当前图像。

VII-B 稀疏直接视觉测量模型

上面提取的视觉地图点 $\displaystyle\{^{G}{\mathbf{p}}_{i}\}$ 用于构建视觉测量模型。其基本原理是，当使用真实状态（即，姿态） $\displaystyle\mathbf{x}_{k}$ 将地图点 $\displaystyle{{}^{G}{\mathbf{p}}}_{i}$ 转换为当前图像 $\displaystyle\mathbf{I}_{k}(\cdot)$ 时，参考补丁和当前补丁之间的光度误差应为零：

\displaystyle\displaystyle\begin{split}~{}\mathbf{0}&=\tau_{k}\mathbf{I}_{k}^{% gt}(\underbrace{\bm{\pi}({{}^{C}\mathbf{T}_{I}}{(^{G}\mathbf{T}_{I})^{% \scriptstyle-1}}{{}^{G}\mathbf{p}_{i}})}_{\mathbf{u}_{i}}+\Delta\mathbf{u})\\ &\ \ \ \ \ \ \ \ -\tau_{r}\mathbf{I}_{r}^{gt}(\underbrace{\bm{\pi}({{}^{C_{r}}% \mathbf{T}_{G}}{{}^{G}\mathbf{p}_{i}})}_{\mathbf{u}^{\prime}_{i}}+\mathbf{A}^{% r}_{i}\Delta\mathbf{u})\end{split}

(21)

其中 $\displaystyle\bm{\pi}(\cdot)$ 是常见的相机投影模型（例如，针孔、MEI、ATAN、Scaramuzza、等距）， ${}^{C_{r}}\mathbf{T}_{G}$ 是全局坐标系 $\displaystyle G$ 相对于参考坐标系 $\displaystyle C_{r}$ 的姿态，该姿态在接收和融合参考坐标系时已经估计出来， $\displaystyle\mathbf{A}^{r}_{i}$ 是仿射扭曲矩阵，将像素从第 $\displaystyle i$ 个当前块变换到参考块， $\displaystyle\Delta\mathbf{u}$ 是相对于当前块中心 $\displaystyle\mathbf{u}_{i}$ 的相对像素位置， $\displaystyle\mathbf{I}_{k}^{gt},\mathbf{I}_{r}^{gt}$ 分别表示参考帧和当前帧的真实像素值。它们被测量为实际图像像素值 $\displaystyle\mathbf{I}_{k},\mathbf{I}_{r}$ ，带有测量噪声 $\displaystyle\mathbf{v}_{c}\!=\!(\bm{\delta}{\mathbf{I}}_{k},\bm{\delta}{% \mathbf{I}}_{r})$ ，这些噪声源于各种来源（例如，散粒噪声和相机 CMOS 的模数转换器 (ADC) 噪声）。因此，

\displaystyle\displaystyle\underbrace{\mathbf{0}}_{\mathbf{y}_{c}}=\underbrace% {\tau_{k}(\mathbf{I}_{k}({\mathbf{u}_{i}}\!+\!\Delta\mathbf{u})\!-\!\bm{\delta% }{\mathbf{I}_{k}})\!-\!\tau_{r}(\mathbf{I}_{r}(\mathbf{u}^{\prime}_{i}\!+\!% \mathbf{A}^{r}_{i}\Delta\mathbf{u})\!-\!\bm{\delta}{\mathbf{I}_{r}})}_{\mathbf% {h}_{c}(\mathbf{x},\mathbf{v}_{c})}

(22)

为了提高计算效率，我们采用了一种反向合成公式 [48, 4]，其中参数化 ${}^{G}\mathbf{T}_{I}=^{G}\!\!\widehat{\mathbf{T}}^{\kappa}_{I}\text{Exp}(\bm{% \delta}\mathbf{T})$ 的姿态增量 $\displaystyle\bm{\delta}\mathbf{T}\in\mathbb{R}^{6}$ 在 $\displaystyle\mathbf{u}_{i}$ 中（见 (21)），从 $\displaystyle\mathbf{u}_{i}$ 移动到 $\displaystyle\mathbf{u}^{\prime}_{i}$ ，如下所示：

\displaystyle\displaystyle\begin{split}~{}\mathbf{u}_{i}&=\bm{\pi}({{}^{C}% \mathbf{T}_{I}}{(^{G}\widehat{\mathbf{T}}^{\kappa}_{I})^{\scriptstyle-1}}{{}^{% G}\mathbf{p}_{i}})\\ \mathbf{u}^{\prime}_{i}&=\bm{\pi}({{}^{C_{r}}\mathbf{T}_{G}}\,\text{Exp}(\bm{% \delta}\mathbf{T})^{G}\mathbf{p}_{i})\end{split}

(23)

鉴于参考坐标系中的 $\displaystyle\mathbf{u}^{\prime}_{i}$ 在每次迭代中保持不变，我们只需要对 $\displaystyle\bm{\delta}\mathbf{T}$ 进行一次雅可比矩阵的计算，而不需要在每次迭代中重新计算它们。

为了从测量方程 (22) 中估计反向曝光时间 $\displaystyle\tau_{k}$ ，我们将初始反向曝光时间 $\displaystyle\tau_{0}=1$ 固定，以消除当所有反向曝光时间都为零时方程 (22) 的退化。因此，后续帧的估计反向曝光时间是相对于第一帧的曝光时间。

方程 (22) 用于跨三个级别（见算法 1）的视觉更新步骤；视觉更新从最粗糙的级别开始，在一个级别收敛后，它会继续到下一个更精细的级别。然后使用估计的状态来生成视觉地图点（第 V-C 节）并更新参考块（第 V-D 节）。

VIII 评估数据集

在本节中，我们将介绍用于性能评估的数据集，包括公共数据集 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 节）。

VIII-A NTU-VIRAL、Hilti 和 MARS-LVIG 数据集

NTU-VIRAL 数据集是在南洋理工大学校园使用空中平台收集的，它呈现出各种场景，体现了独特的空中运行挑战。具体来说，“sbs”序列只能提供来自远处物体的嘈杂视觉特征。 “nya”序列由于半透明表面而对 LiDAR SLAM 构成挑战，并且由于复杂的飞行动力学和低光照条件而对视觉 SLAM 构成挑战。该数据集配备了一个 16 通道 OS1 gen1³³3https://ouster.com/products/os1-lidar-sensor LiDAR，以 10 Hz 的频率采样，并配有一个以 100 Hz 的频率运行的内置 IMU，以及两个以 10 Hz 的频率触发的同步针孔相机。左侧相机用于评估。

Hilti’22 和 Hilti’23 数据集是通过手持设备和机器人设备收集的，涵盖了建筑工地、办公室、实验室和停车场等环境的室内和室外序列。这些序列从长走廊、地下室和楼梯中引入了许多挑战，具有无纹理特征、不同的照明条件和不足的 LiDAR 平面约束。手持序列使用 Hesai PandarXT-32⁴⁴4https://www.hesaitech.com/product/xt32/ LiDAR 以 10 Hz 的频率运行，五个广角相机以 40 Hz 的频率运行（降采样到 10 Hz），以及一个以 400 Hz 的频率运行的外部 Bosch BMI085 IMU。同时，机器人安装的序列采用 Robosense BPearl⁵⁵5https://www.robosense.ai/en/rslidar/RS-Bpearl LiDAR 以 10 Hz 的频率运行，八个全向相机以 10 Hz 的频率运行，以及一个以 200 Hz 的频率运行的 Xsens MTi-670 IMU。在这两种情况下，所有正在评估的系统都使用面向正面的相机。每个序列都提供了通过运动捕捉系统 (MoCap) 或全站仪[54] 获得的毫米级精确地面实况。请注意，Hilti 数据集的地面实况不是开源的；因此，这些数据集上的算法结果是通过 Hilti 官方网站进行评估的。由于 Hilti’23 中的“Site 3”未提供深入的分析图（例如，RMSE），因此我们排除了这四个序列，但我们对这些序列的评分结果仍然可以在其官方网站上找到⁶⁶6https://hilti-challenge.com/leader-board-2022.html, https://hilti-challenge.com/leader-board-2023.html。 NTU-VIRAL 和 Hilti 共贡献了 25 个序列。

MARS-LVIG 数据集提供了高空、地面朝向的测绘数据，涵盖了丛林、山脉和岛屿等各种非结构化地形。该数据集是通过 DJI M300 RTK 四旋翼无人机收集的，该无人机配备了 Livox Avia⁷⁷7https://www.livoxtech.com/avia LiDAR（内置 BMI088 IMU）和一个高分辨率全局快门相机，两者都以 10 Hz 的频率触发。这与上述 NTU-VIRAL 和 Hilti 数据集明显不同，后者使用 $\displaystyle 752\times 480$ 灰度图像，而 MARS 数据集则使用 $\displaystyle 2448\times 2048$ RGB 图像，从而有助于生成清晰、密集的彩色点云。因此，我们利用这个公共数据集来验证我们在高空航空测绘应用中的能力。

VIII-B FAST-LIVO2 私有数据集

为了验证系统在更极端条件下的性能（例如，LiDAR 退化、低照度、大幅度曝光变化和没有 LiDAR 测量的情况），我们创建了一个名为 FAST-LIVO2 私有数据集。该数据集、硬件设备和硬件同步方案已与本工作的代码一起发布，以方便重现我们的工作。

VIII-B1 平台

我们的数据采集平台，如图 8 所示，配备了一台工业相机 (MV-CA013-21UC)、一台 Livox Avia LiDAR 和一台 DJI manifold-2c（英特尔 i7-8550u CPU 和 8 GB 内存）作为机载计算机。相机 FoV 为 $\displaystyle 70.6^{\circ}\times 68.5^{\circ}$ ，LiDAR FoV 为 $\displaystyle 70.4^{\circ}\times 77.2^{\circ}$ 。所有传感器都通过 STM32 同步定时器产生的 10 Hz 触发信号进行硬件同步。

VIII-B2 序列描述

如补充材料[53]中表 S1 所总结，FAST-LIVO2 私有数据集包含跨越各种场景（例如，校园建筑、走廊、地下室、采矿隧道等）的 20 个序列，这些序列的特点是结构松散、杂乱、昏暗、照明变化以及纹理弱的环境，总时长为 66.9 分钟。大多数序列表现出视觉和/或 LiDAR 退化，例如面对单个和/或无纹理的平面、穿越极其狭窄和/或黑暗的隧道，以及经历从室内到室外的不同光照条件（见补充材料[53]中的图 S7）。为了保证相机和 LiDAR 之间同步数据采集的增强，我们在大多数情况下将相机配置为固定曝光时间，但自动增益模式。对于剩余的具有自动曝光的序列，我们记录它们的真实曝光时间。在所有序列中，平台都返回到起点，这使得可以进行漂移评估。

IX 实验结果

在本节中，我们进行了大量的实验来评估我们提出的系统。

IX-A 实现和系统配置

我们在 C++ 和机器人操作系统 (ROS) 中实现了提出的 FAST-LIVO2 系统。在默认配置中，曝光时间估计被启用，而法向量细化被关闭。扫描中的 LiDAR 点以 1:3 的比例在时间上进行下采样。体素地图的根体素大小设置为 0.5 m，内部八叉树的最大层数为 3。图像块大小为 $\displaystyle 8\times 8$ 用于图像对齐， $\displaystyle 11\times 11$ 用于法向量细化。在顺序 ESIKF 设置中，对于所有实验，相机光度噪声被设置为 100 的恒定值。 LiDAR 深度误差和方位角误差分别调整为 Livox Avia LiDAR 和 OS1-16 的 0.02 m 和 0.05°，PandarXT-32 的 0.001 m 和 0.001°，Robosense BPearl LiDAR 的 0.008 m 和 0.01°。 Livox Avia LiDAR 和 OS1-16 的激光束发散角设置为 0.15°，PandarXT-32 和 Robosense BPearl LiDAR 的激光束发散角设置为 0.001°。我们的系统在所有数据集的所有序列中使用相同的参数，前提是传感器设置相同。所有实验的计算平台都是配备了 Intel i7-10700K CPU 和 32 GB RAM 的台式 PC。对于 FAST-LIVO2，我们还在 ARM 处理器上对其进行了测试，该处理器通常用于嵌入式系统，并具有降低的功耗和成本。 ARM 平台是 RB5⁸⁸8https://www.adlinktech.com/Products/Computer_on_Modules/SMARC/LEC-RB5?lang=en。配备 Qualcomm Kryo585 CPU 和 8 GB RAM。我们将 FAST-LIVO2 在基于 ARM 的平台上的实现称为“FAST-LIVO2（ARM）”。

表 II：序列中的绝对平移误差（RMSE，米）

Dataset	Sequence	SDV- LOAM	Our LIO	FAST- LIO2	R3LIVE	LVI- SAM	FAST- LIVO	Ours (w/o expo)	Ours (w normal)	Ours (w/o update)	Ours
Hilti’22	Construction Ground	25.121	0.011	0.013	0.021	$\displaystyle\times$	0.022	0.011	0.008	0.015	0.010
	Construction Multilevel	12.561	0.031	0.044	0.024	$\displaystyle\times$	0.052	0.021	0.018	0.025	0.020
	Construction Stairs	9.212	0.221	0.320	0.784	9.142	0.241	0.049	0.027	0.151	0.016
	Long Corridor	19.531	0.061	0.064	0.061	6.312	0.065	0.069	0.059	0.071	0.067
	Cupola	9.321	0.221	0.250	2.142	$\displaystyle\times$	0.182	0.161	0.122	0.179	0.121
	Lower Gallery	11.232	0.014	0.024	0.008	2.281	0.022	0.010	0.008	0.010	0.007
	Attic to Upper Gallery	4.551	0.223	0.720	2.412	$\displaystyle\times$	0.621	0.101	0.077	0.221	0.069
	Outside Building	2.622	0.030	0.028	0.029	0.952	0.052	0.042	0.033	0.050	0.035
Hilti’23	Floor 0	4.621	0.028	0.031	0.018	$\displaystyle\times$	0.021	0.025	0.023	0.023	0.022
	Floor 1	7.951	0.025	0.031	0.024	8.682	0.022	0.024	0.022	0.031	0.023
	Floor 2	7.912	0.041	0.083	0.046	$\displaystyle\times$	0.048	0.023	0.021	0.051	0.022
	Basement	6.151	0.021	0.038	0.024	$\displaystyle\times$	0.035	0.020	0.018	0.018	0.016
	Stairs	9.032	0.110	0.170	0.110	3.584	0.152	0.025	0.020	0.132	0.018
	Parking 3x floors down	19.952	0.162	0.320	0.462	$\displaystyle\times$	0.356	0.035	0.022	0.112	0.032
	Large room	16.781	0.121	0.028	0.035	0.563	0.031	0.033	0.027	0.118	0.026
	Large room (dark)	15.012	0.051	0.040	0.059	$\displaystyle\times$	0.053	0.049	0.051	0.058	0.046
NTU VIRAL	eee_01	0.301	0.122	0.212	0.072	3.901	0.191	0.069	0.066	0.109	0.068
	eee_02	1.842	0.131	0.172	0.059	0.182	0.132	0.051	0.055	0.112	0.051
	eee_03	0.301	0.124	0.213	0.078	0.287	0.192	0.068	0.070	0.099	0.068
	nya_01	0.202	0.084	0.141	0.080	0.205	0.121	0.075	0.078	0.106	0.073
	nya_02	0.214	0.153	0.212	0.084	1.296	0.182	0.076	0.081	0.118	0.075
	nya_03	0.251	0.082	0.133	0.079	0.176	0.112	0.060	0.060	0.092	0.059
	sbs_01	0.212	0.112	0.184	0.075	0.254	0.253	0.064	0.063	0.098	0.062
	sbs_02	0.233	0.123	0.161	0.076	0.221	0.134	0.062	0.048	0.116	0.061
	sbs_03	0.281	0.122	0.142	0.070	0.309	0.132	0.061	0.047	0.119	0.060
Average		7.416	0.097	0.151	0.278	1.928	0.137	0.051	0.044	0.089	0.045

1

$\displaystyle\times$ 表示系统完全失败。

IX-B 基准实验

在此实验中，我们对来自 NTU-VIRAL、Hilti’22 和 23 个开放数据集的 25 个序列进行了定量评估。我们的方法与几种最先进的开源里程计系统进行了基准测试，包括 R3LIVE [36]，一种密集直接的 LiDAR-惯性-视觉里程计系统；FAST-LIO2 [13]，一种直接的 LiDAR-惯性里程计系统；SDV-LOAM [40]，一种半直接的 LiDAR-视觉里程计系统；LVI-SAM [35]，一种基于特征的 LiDAR-惯性-视觉 SLAM 系统；以及我们之前的工作 FAST-LIVO [8]。

这些系统是从它们各自的 GitHub 存储库下载的。对于 FAST-LIO2、FAST-LIVO 和 LVI-SAM，我们使用推荐的设置，适用于配备多线 LiDAR 传感器的室内和室外场景。为了使 R3LIVE 适应系统，我们调整了系统使其能够使用鱼眼相机模型和配备外部 IMU 的多线 LiDAR（默认配置仅支持内部 IMU）。由于数据集中的 IMU 激励不足导致优化不利，我们禁用了相机内参和外参的实时优化 ${}^{C}\mathbf{T}_{I}$ 。其他参数，包括光流跟踪的窗口大小和金字塔级别、对当前扫描和全局地图进行降采样的点云的分辨率，都经过了微调以实现最佳性能。由于 SDV-LOAM 只有视觉模块是开源的，我们将其与 LeGO-LOAM[7] 集成，并采用松耦合的方式，遵循原始论文[40]中描述的方法。此增强型系统继续细化从视觉模块获得的姿态，我们也将在 GitHub⁹⁹9https://github.com/xuankuzcr/SDV-LOAM_reimplementation 上公开此实现。鉴于所有比较的系统都是没有闭环的里程计，除了 LVI-SAM 之外，我们去掉了 LVI-SAM 的闭环模块，以确保公平的比较。此外，我们对曝光时间估计模块、法线细化模块和参考补丁更新策略进行了消融研究。默认的 FAST-LIVO2 具有实时曝光估计和参考补丁更新，但没有法线细化。

所有方法的结果如表 II 所示。可以看出，我们的方法在所有序列中都获得了最高的总体精度，平均 RMSE 为 0.044 m，比第二名的 FAST-LIVO 的 0.137 m 精确三倍。除了“建筑物外部”和“大房间（黑暗）”序列，我们的系统在大多数序列中都获得了最佳结果，其中我们的系统与仅基于 LiDAR-惯性的里程计 FAST-LIO2 相比，表现出略微（毫米级）更高的误差。这种差异可归因于这些序列丰富的结构特征但光照条件较差，导致图像昏暗且模糊。因此，融合这些低质量图像不会提高里程计精度。排除这两个序列后，我们的方法利用紧耦合的 LiDAR、惯性和视觉信息，显著优于 FAST-LIO2（我们的 LIO 子系统）和仅基于 LiDAR-视觉的里程计 SDV-LOAM。值得注意的是，SDV-LOAM 在 Hilti 数据集上的表现特别差，因为它没有与 IMU 测量值紧密集成，导致 LO 子系统出现漂移。此外，LiDAR 和视觉观测之间的松耦合，以及 VO 的较差初始值，往往会导致局部最优甚至负优化。我们的 LIO 子系统通常优于 FAST-LIO2，因为我们对每个 LiDAR 点进行了更精确的噪声建模。在少数 FAST-LIO2 表现略优的序列中，差异微乎其微，仅在毫米级，可以忽略不计。此外，我们的系统精度在所有序列中均显著超过其他紧耦合 LiDAR-惯性-视觉系统。其中，LVI-SAM 在九个序列中失效，主要原因是其基于特征的 LIO 和 VIO 子系统没有充分利用原始测量值，这降低了其在具有细微几何或纹理特征的环境中的鲁棒性。 R3LIVE 通常表现良好，但在“施工楼梯”、“圆顶”和“阁楼至上层画廊”序列中存在困难，其性能甚至比 FAST-LIO2 更差。这是因为在无结构楼梯处进行剧烈旋转会导致姿势先验不足，在将彩色地图点与当前帧对齐时导致局部最优，最终导致负优化。 FAST-LIVO 和 FAST-LIVO2 通过基于补丁的图像对齐克服了这些序列中的这些挑战。此外，在这些序列中传感器靠近墙壁的情况突出了 FAST-LIVO2 中射线投射的有效性，这些大型场景的映射结果如图 S8 所示，见补充材料 [53]。另一方面，在 NTU-VIRAL 数据集中，FAST-LIVO 的表现不如 R3LIVE 和 FAST-LIVO2，尤其是在“nya”序列等非结构化场景中，基于恒定深度假设的仿射扭曲的影响不准确。相反，R3LIVE 的像素级对齐和 FAST-LIVO2 的平面先验（或细化）不会遇到此类问题。

比较 FAST-LIVO2 的不同变体，我们观察到，没有实时曝光时间估计的平均精度与默认值相比下降了 6 毫米，因为曝光时间估计可以主动补偿环境中的照明变化。另一方面，与默认值相比，没有参考补丁更新的平均精度下降了 44 毫米，因为参考补丁更新策略有效地选择了分辨率更高的补丁，并避免了选择异常值补丁。最后，法线细化将平均精度提高了 1 毫米，但精度提高并非在所有序列中都一致。改善幅度有限的主要原因是，法线矢量细化仅在具有良好图像观测的简单结构化场景中才会产生正优化。在 NTU-VIRAL 数据集中，“eee”和“nya”序列中的图像非常昏暗模糊，负优化尤其严重。为了进一步研究不同模块的有效性，包括曝光时间估计、仿射扭曲、参考补丁更新、法线收敛、按需射线投射和 ESIKF 顺序更新，我们在私有数据集和 MARS-LVIG 数据集上进行了深入研究。由于空间限制，结果在补充材料 [53] 的第一部分（系统模块验证）中展示。正如结果所证实的那样，我们的系统可以在结构化和非结构化环境中、在严重的灯光变化下、在具有长期、高速数据采集的极大规模场景中，甚至在 LiDAR 测量很少的极窄空间中实现稳健且准确的姿态估计。

IX-C LiDAR 退化和视觉挑战环境

在此实验中，我们评估了系统在经历 LiDAR 退化和/或视觉挑战的环境中的鲁棒性，并将该系统与 FAST-LIVO 和 R3LIVE 在 8 个序列中的定性映射结果进行了比较，如图 9 和 10 所示。图 9 展示了 LiDAR 退化序列，其中 LiDAR 面对着一堵大墙，并从一侧沿墙移动到另一侧。由于 LiDAR 仅观察到一个墙面，因此不存在几何约束，LIO 方法将失败。值得一提的是，“HIT 涂鸦墙”序列跨越近 800 米，LiDAR 不断面向墙壁，导致相当大的退化。在所有序列中，FAST-LIVO2 明显展示了其对长期退化的鲁棒性，以及其提供高精度彩色点地图的能力。相反，FAST-LIVO 设法获得了几何结构，但纹理完全模糊。 R3LIVE 在几何结构和纹理清晰度方面都存在问题。图 10 展示了在更复杂场景下的测试，其中 LiDAR 和/或摄像头偶尔会出现退化。退化方向由相应的箭头指示。 “香港大学文化中心” (图 10 (a)) 展示了 FAST-LIVO2、R3LIVE 和 FAST-LIVO 的映射结果。可以看出，R3LIVE 和 FAST-LIVO 具有扭曲的点云，模糊的纹理，以及超过 1 米的漂移。相反，FAST-LIVO2 成功返回起点，实现了不到 0.01 米的令人印象深刻的端到端误差，同时实现了具有清晰纹理的一致点云。 “CBD 大厦 03” (图 10 (b)) 和 “采矿隧道” (图 10 (c)) 仅显示 FAST-LIVO2 的结果，因为 R3LIVE 和 FAST-LIVO 失败了。在图 10 (b) 中，蓝色箭头表示向纯黑屏幕移动，表示 LiDAR 和摄像头同时退化。在图 10 (c1) 和 (c2) 中，红色点代表该位置的 LiDAR 扫描，说明了由于观察到单个平面而导致的 LiDAR 退化区域。此外，整个序列中“采矿隧道” 的照明非常昏暗，并且视觉和 LiDAR 经常退化。尽管存在这些挑战，FAST-LIVO2 仍然在两个序列中返回起点，端到端误差小于 0.01 米。

IX-D 高精度映射

在本实验中，我们验证了我们系统的高精度映射能力。为了探索不同算法的映射精度并确保公平性，我们在以丰富纹理和结构化环境为特征的场景中，将我们的系统与 FAST-LIO2、R3LIVE 和 FAST-LIVO 进行比较。我们以“SYSU 01”、 “HKU 地标” 和 “CBD 大厦 01” 序列为例。图 S9 在补充材料[53] 中展示了这些序列实时重建的彩色点云。我们可以清楚地观察到，FAST-LIVO2 生成的点云地图在所有系统中保留了最精细的细节，彩色点云地图的放大视图类似于实际 RGB 图像中的视图。在 “SYSU 01” 序列中，我们的算法在告示牌上产生的白噪声点更少，因为我们在着色之前使用恢复的曝光时间将图像颜色归一化为合理的曝光时间，从而导致很少出现过曝的彩色点云地图。 “CBD Building 01” 中人类和摩托车的重建也体现了我们重建非结构化物体细节的能力。在所有序列中，估计的最终位置都返回到起点，端到端误差小于 0.01 米。我们还在私有数据集的剩余序列中测试了 FAST-LIVO2，映射结果如图 S10-S13 所示，见补充材料[53]。

IX-E 运行时间分析

本节评估了我们提出的系统在配备英特尔 i7-10700K CPU 和 32 GB RAM 的台式电脑上测试的每个 LiDAR 扫描和图像帧的平均计算时间。我们的评估涵盖了公共数据集，包括 Hilti’22、Hilti’23 和 NTU-VIRAL，以及我们的私有数据集。如表 III 所示，我们的系统在所有序列中表现出最低的处理时间。在英特尔 i7 处理器上的平均计算时间消耗仅为 30.03 毫秒（每个 LiDAR 扫描 17.13 毫秒，每个图像帧 12.90 毫秒），满足 10 Hz 的实时操作。此外，我们的系统甚至可以在 ARM 处理器上以每帧 78.44 毫秒的平均处理时间实现实时操作。 LVI-SAM 的 LIO 和 VIO 中的 LiDAR 和视觉特征提取模块非常耗时。除了 LIO 和 VIO 消耗的时间外，LVI-SAM 还将 IMU 预积分约束、视觉里程计约束和 LiDAR 里程计约束集成到因子图中，进一步增加了整体处理时间。对于 R3LIVE，尽管也采用直接方法，但其逐像素图像对齐需要使用大量视觉地图点。相反，我们的方法使用稀疏点和参考块，从而实现高效的对齐。此外，R3LIVE 维护一个彩色地图，该地图进行贝叶斯更新，随着地图分辨率的提高，计算量显著增加。对于 FAST-LIO2，由于空间限制，每帧的平均处理时间（表 S3，见补充材料[53]）比 FAST-LIVO2 少约 10.35 毫秒，因为没有处理额外的图像测量。

FAST-LIVO2 也比其前身 FAST-LIVO 有了显著的改进。主要改进来自于我们在稀疏图像对齐中应用了逆合成公式。利用基于 LiDAR 点的平面先验进行仿射扭曲进一步提高了我们方法的收敛效率。因此，FAST-LIVO2 将每个金字塔层的迭代次数从 $\displaystyle 10$ 减少到 $\displaystyle 3$ ，同时仍能达到更高的精度。

表 III：每个 LiDAR 和图像帧的处理时间 (ms)

Dataset	R3LIVE	LVI- SAM	FAST- LIVO	FAST-LIVO2 (LiDAR / Image)	FAST-LIVO2 (ARM)
Hilti’22
Construction Ground	105.03	$\displaystyle\times$	52.33	36.52 (20.44 / 15.05)	96.12
Construction Multilevel	112.13	$\displaystyle\times$	56.12	38.77 (21.38 / 17.39)	95.38
Construction Stairs	125.41	138.47	51.34	39.33 (24.32 / 15.01)	98.43
Long Corridor	120.42	109.97	48.68	41.42 (26.21 / 15.21)	94.33
Cupola	151.52	$\displaystyle\times$	59.42	43.54 (26.53 / 17.01)	98.12
Lower Gallery	119.74	131.37	51.15	41.11 (25.69 / 15.42)	92.13
Attic to Upper Gallery	144.39	$\displaystyle\times$	58.61	44.21 (27.12 / 17.09)	91.15
Outside Building	105.17	107.92	44.25	33.82 (18.91 / 14.91)	85.43
Hilti’23
Floor 0	117.01	$\displaystyle\times$	52.23	42.22 (25.13 / 17.09)	92.24
Floor 1	106.11	106.98	50.14	43.12 (27.43 / 15.69)	93.52
Floor 2	154.65	$\displaystyle\times$	53.24	41.78 (26.12 / 15.66)	94.43
Basement	118.21	$\displaystyle\times$	48.23	39.65 (24.53 / 15.12)	93.42
Stairs	122.94	114.29	48.55	38.42 (22.64 / 15.78)	95.53
Parking 3x floors down	142.43	$\displaystyle\times$	51.89	43.62 (26.99 / 16.63)	94.22
Large room	125.78	182.18	55.23	43.23 (26.43 / 16.80)	93.28
Large room (dark)	131.31	$\displaystyle\times$	51.46	44.21 (29.12 / 15.09)	92.29
NTU VIRAL
eee_01	105.89	113.61	38.22	31.45 (17.24 / 14.21)	78.03
eee_02	112.27	119.19	39.43	30.24 (16.23 / 14.01)	79.22
eee_03	108.11	108.77	37.53	29.44 (16.12 / 13.32)	77.43
nya_01	122.73	124.54	39.66	34.52 (18.14 / 16.38)	79.44
nya_02	111.96	117.18	35.11	32.23 (17.33 / 14.90)	80.52
nya_03	115.96	111.24	38.42	33.42 (18.12 / 15.30)	78.95
sbs_01	112.77	115.67	39.23	31.92 (14.68 / 17.24)	78.32
sbs_02	113.01	110.37	35.39	32.56 (17.62 / 14.94)	72.43
sbs_03	119.91	120.73	37.41	33.62 (18.02 / 15.60)	75.56
Private Dataset
Retail Street	85.32	70.23	31.22	19.44 (10.42 / 9.02)	64.12
CBD Building 01	79.22	65.32	29.14	18.22 (10.32 / 7.90)	62.33
CBD Building 02	$\displaystyle\times$	$\displaystyle\times$	$\displaystyle\times$	21.53 (11.50 / 10.03)	69.98
CBD Building 03	$\displaystyle\times$	$\displaystyle\times$	$\displaystyle\times$	21.89 (11.98 / 9.91)	69.92
HKU Landmark	75.43	68.77	28.33	17.42 (10.12 / 7.30)	63.21
HKU Lecture Center	70.42	$\displaystyle\times$	31.45	18.12 (10.99 / 7.13)	62.88
HKU Centennial Garden	74.33	70.12	33.46	19.22 (10.80 / 8.42)	64.52
HKU Cultural Center	102.34	$\displaystyle\times$	35.62	21.43 (11.42 / 10.01)	72.43
HKU Main Building	75.62	$\displaystyle\times$	30.21	19.43 (10.33 / 9.10)	65.31
HKUST Red Sculpture	84.22	90.13	32.43	20.14 (11.12 / 9.02)	65.42
HIT Graffiti Wall	112.56	$\displaystyle\times$	39.98	21.71 (10.55 / 11.16)	68.99
Banner Wall	74.13	$\displaystyle\times$	30.22	19.32 (10.01 / 9.31)	63.43
Bright Screen Wall	73.23	$\displaystyle\times$	28.43	18.22 (9.92 / 8.30)	61.21
Black Screen Wall	71.42	$\displaystyle\times$	27.66	17.99 (9.10 / 8.89)	60.91
Office Building Wall	$\displaystyle\times$	$\displaystyle\times$	$\displaystyle\times$	19.42 (10.11 / 9.31)	62.33
Narrow Corridor	$\displaystyle\times$	$\displaystyle\times$	$\displaystyle\times$	18.44 (8.32 / 10.12)	61.42
Long Corridor	$\displaystyle\times$	$\displaystyle\times$	$\displaystyle\times$	19.53 (10.43 / 9.10)	62.55
Mining Tunnel	$\displaystyle\times$	$\displaystyle\times$	$\displaystyle\times$	29.43 (15.42 / 14.01)	79.32
SYSU 01	112.66	88.95	32.23	23.65 (12.51 / 11.14)	77.43
SYSU 02	110.12	$\displaystyle\times$	32.12	22.63 (11.82 / 10.81)	72.31
Average	108.36	108.45	41.43	30.03 (17.13 / 12.90)	78.44

1

$\displaystyle\times$ 表示系统完全失败。

X 应用

为了展示 FAST-LIVO2 在实际应用中的优异性能和多功能性，我们开发了多种解决方案，包括全自主无人机导航、航空测绘、纹理网格生成以及 3D 高斯散射重建用于 3D 场景表示。

X-A 全方位自主无人机导航

鉴于 FAST-LIVO2 具有高精度和稳健的定位性能，以及其实时能力，我们进行了闭环自主无人机飞行。

X-A1 系统配置

硬件和软件设置如图 11 所示。在硬件方面，我们使用 NUC（英特尔 i7-1360P CPU 和 32 GB RAM）作为机载计算机。在软件方面，定位组件由 FAST-LIVO2 提供支持，其以 10Hz 的频率提供位置反馈。定位结果被馈送到飞行控制器，以实现 200Hz 的位置、速度和姿态反馈。除了定位之外，FAST-LIVO2 还向规划模块提供密集的注册点云，即 Bubble 规划器 [55]，该规划器规划一条平滑的轨迹，然后由一个基于流形的模型预测控制 (MPC) [56] 跟踪。 MPC 计算所需的角速度和推力，这些值由在飞行控制器上运行的相应的低级角速度控制器跟踪。重要的是，MPC、规划器和 FAST-LIVO2 都在机载计算机上实时运行。

X-A2 无人机自主导航

我们进行了 4 个全方位自主无人机导航实验，“地下室”、“树林”、“狭窄通道”和“SYSU 校园”（补充材料中表格 S2 [53]）。 “地下室”和“树林”实验是完全自主的飞行，包含所有规划、MPC 和 FAST-LIVO2 模块，而“狭窄通道”和“SYSU 校园”是手动飞行，只有 MPC 和 FAST-LIVO2（没有规划组件）。如图所示，“地下室”和“树林”展示了无人机成功的自主导航和避障能力。在“狭窄通道”中，无人机被命令沿着墙壁飞行，距离墙壁很近，导致很少的激光雷达点测量。然而，射线投射模块召回了更多数量的视觉地图点，为定位提供了充足的约束，从而实现了稳定的定位。此外，“地下室”和“狭窄通道”经历了激光雷达退化，仅观察到一个墙壁（见图 LABEL:fig_cover (e1) 和 (e4)，图 12 (b1-b4)），以及显著的曝光变化（见图 LABEL:fig_cover (e5-e6)）。尽管存在这些挑战，我们的无人机系统表现得非常出色。 “树林”涉及无人机以高达 3 m/s 的高速移动，要求整个无人机系统快速响应（见图 12 (a1-a4)）。 “SYSU 校园”是一个非退化场景，主要展示了机载高精度测绘能力（见补充材料[53]中的图 S14）。最后，值得一提的是，在这四次无人机飞行中都发生了严重的照明变化。 FAST-LIVO2 能够估计与地面真实值非常接近的曝光时间（见补充材料[53]中的图 S15）。

关于机载计算时间，在机载计算机上运行 MPC（以 100 Hz）和规划（以 10 Hz）需要消耗计算资源和内存，限制了 FAST-LIVO2 可用的计算资源。尽管同时执行控制和规划，如图 13 所示，FAST-LIVO2 每个激光雷达扫描和图像帧的平均机载处理时间约为 53.47 毫秒，仍然远低于帧周期 100 毫秒。规划和 MPC 的平均处理时间分别为 8.43 毫秒和 18.5 毫秒。总的平均处理时间为 80.4 毫秒，很好地满足了机载操作的实时要求。

X-B 航空测绘

航空测绘是测绘应用中的一个关键任务。为了评估 FAST-LIVO2 在此应用中的适用性，我们使用公共数据集 MARS-LVIG [52] 进行了一次航拍实验，其硬件配置在第 VIII-A 节中详细说明。我们评估了两个序列“HKairport01”和“HKisland01”，其实时映射结果如图 LABEL:fig_cover (a-c) 所示，其中 (a) 和 (c) 对应于“HKisland01”，而 (b) 描述了“HKairport01”。结果证明了 FAST-LIVO2 在森林和岛屿等非结构化环境中的有效性。该系统成功捕获了许多精细结构和鲜明的色彩效果，包括建筑物、道路上的车道标记、路缘石、树冠和岩石，所有这些都清晰可见。这些序列的 APE (RMSE) 分别为 FAST-LIVO2 的 0.64 米和 0.27 米，而 R3LIVE 的分别为 2.76 米和 0.52 米。在台式 PC 上的平均处理时间（第 IX-A 节）分别约为 25.2 毫秒和 21.8 毫秒，而 R3LIVE 的分别为 110.5 毫秒和 100.2 毫秒。

X-C 支持 3D 场景应用：网格生成、纹理和高斯散射

利用从 FAST-LIVO2 获得的高精度传感器定位和密集的 3D 彩色点云地图，我们开发了用于渲染管线的软件应用程序，包括网格化和纹理化，以及新兴的类似 NeRF 的渲染管线，例如 3D 高斯散射 (3DGS)。对于网格化，我们采用基于截断符号距离函数 (TSDF) 的 VDBFusion [57]，如图 14 (a) 所示，位于“CBD 大厦 01”。柱子上的锋利边缘和屋顶的独特结构清晰可见，证明了网格的高质量。这种细节级别是由于 FAST-LIVO2 点云的高密度和结构重建的卓越精度而实现的。网格构建完成后，我们使用 OpenMVS [58] 通过估计的相机姿态进行纹理映射，位于“CBD 大厦 01”和“零售街”，如图 14 (b-c) 所示。在图 14 (c1-c2) 中，应用于三角形面的纹理图像无缝且准确地对齐，从而产生了高度清晰和精确的纹理映射。这是由于 FAST-LIVO2 实现的像素级图像对齐。

来自 FAST-LIVO2 的密集彩色点云也可以直接作为 3DGS 的输入。我们利用 300 帧（总共 1,180 张图像）在序列“CBD 大厦 01”上进行测试。结果如图 15 所示。与 COLMAP[59] 相比，我们的方法显着减少了从密集点云和姿态获取所需的时间，从 9 小时缩短到 21 秒。但是，训练时间从 10 分钟 59 秒增加到 15 分钟 30 秒。这种增加归因于更密集的点云（下采样到 5 厘米），这引入了更多需要优化的参数。尽管如此，我们点云密度的增加和精度的提高导致与从 COLMAP 输入获得的 PSNR 相比，峰值信噪比 (PSNR) 略有提高。

XI 结论和未来工作

本文提出了 FAST-LIVO2，一个直接的 LIVO 框架，它在实时重建地图的同时实现了快速、准确和鲁棒的状态估计。 FAST-LIVO2 可以在保持对严重激光雷达和/或视觉退化具有鲁棒性的同时，实现高定位精度。

速度的提高归因于在高效的 ESIKF 框架中使用原始激光雷达、惯性和相机测量，并进行顺序更新。在图像更新中，进一步采用了逆合成公式和基于稀疏补丁的图像对齐，以提高效率。准确性的提高归功于使用（甚至改进）来自 LiDAR 点的平面先验来提高图像对齐的精度。此外，一个统一的体素地图被用来同时管理地图点和观察到的高分辨率图像测量。体素地图结构支持几何构造和更新、视觉地图点生成和更新以及参考补丁更新，并且得到了开发和验证。鲁棒性的提高归功于实时估计曝光时间，这有效地处理了环境照明的变化，以及按需体素光线投射，以应对 LiDAR 的近距离盲区。 FAST-LIVO2 的效率和准确性在大量公共数据集上进行了评估，而每个系统模块的鲁棒性和有效性在私有数据集上进行了评估。 FAST-LIVO2 在现实世界机器人应用中的应用，如无人机导航、3D 映射和模型渲染，也得到了展示。

作为一种里程计，FAST-LIVO2 在长距离上可能会出现漂移。在未来，我们可以将闭环和滑动窗口优化整合到 FAST-LIVO2 中，以减轻这种长期漂移。此外，准确且密集的彩色点地图可用于提取语义信息以进行物体级语义映射。

参考文献

[1] R. Mur-Artal and J. D. Tardós, “Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras,” IEEE transactions on robotics, vol. 33, no. 5, pp. 1255–1262, 2017.
[2] J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE transactions on pattern analysis and machine intelligence, vol. 40, no. 3, pp. 611–625, 2017.
[3] J. Engel, T. Schöps, and D. Cremers, “Lsd-slam: Large-scale direct monocular slam,” in European conference on computer vision. Springer, 2014, pp. 834–849.
[4] C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, “Svo: Semidirect visual odometry for monocular and multicamera systems,” IEEE Transactions on Robotics, vol. 33, no. 2, pp. 249–265, 2016.
[5] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.” in Robotics: Science and Systems, vol. 2, no. 9, 2014.
[6] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidar odometry and mapping package for lidars of small fov,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 3126–3131.
[7] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 4758–4765.
[8] C. Zheng, Q. Zhu, W. Xu, X. Liu, Q. Guo, and F. Zhang, “Fast-livo: Fast and tightly-coupled sparse-direct lidar-inertial-visual odometry,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022, pp. 4003–4009.
[9] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
[10] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “Orb-slam: a versatile and accurate monocular slam system,” IEEE transactions on robotics, vol. 31, no. 5, pp. 1147–1163, 2015.
[11] M.Irani and P.Anandan, “All about direct methods,” in Proc. Workshop Vis. Algorithms, Theory Pract, 1999, pp. 267–277.
[12] C. Forster, M. Pizzoli, and D. Scaramuzza, “Svo: Fast semi-direct monocular visual odometry,” in 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014, pp. 15–22.
[13] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “Fast-lio2: Fast direct lidar-inertial odometry,” IEEE Transactions on Robotics, pp. 1–21, 2022.
[14] C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry,” IEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 8518–8525, 2022.
[15] M. Meilland, A. I. Comport, and P. Rives, “Real-time dense visual tracking under large lighting variations,” in British Machine Vision Conference. British Machine Vision Association, 2011, pp. 45–1.
[16] T. Tykkälä, C. Audras, and A. I. Comport, “Direct iterative closest point for real-time visual odometry,” in 2011 IEEE International Conference on Computer Vision Workshops (ICCV Workshops). IEEE, 2011, pp. 2050–2056.
[17] C. Kerl, J. Sturm, and D. Cremers, “Robust odometry estimation for rgb-d cameras,” in 2013 IEEE international conference on robotics and automation. IEEE, 2013, pp. 3748–3754.
[18] J. Engel, J. Sturm, and D. Cremers, “Semi-dense visual odometry for a monocular camera,” in Proceedings of the IEEE international conference on computer vision, 2013, pp. 1449–1456.
[19] K. Chen, R. Nemiroff, and B. T. Lopez, “Direct lidar-inertial odometry: Lightweight lio with continuous-time motion correction,” in 2023 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2023, pp. 3983–3989.
[20] Z. Wang, L. Zhang, Y. Shen, and Y. Zhou, “D-liom: Tightly-coupled direct lidar-inertial odometry and mapping,” IEEE Transactions on Multimedia, 2022.
[21] J. Zhang and S. Singh, “Laser–visual–inertial odometry and mapping with high robustness and low drift,” Journal of field robotics, vol. 35, no. 8, pp. 1242–1264, 2018.
[22] W. Shao, S. Vijayarangan, C. Li, and G. Kantor, “Stereo visual inertial lidar simultaneous localization and mapping,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2019, pp. 370–377.
[23] J. Zhang, M. Kaess, and S. Singh, “A real-time method for depth enhanced visual odometry,” Autonomous Robots, vol. 41, pp. 31–43, 2017.
[24] J. Graeter, A. Wilczynski, and M. Lauer, “Limo: Lidar-monocular visual odometry,” in 2018 IEEE/RSJ international conference on intelligent robots and systems (IROS). IEEE, 2018, pp. 7872–7879.
[25] Y. Zhu, C. Zheng, C. Yuan, X. Huang, and X. Hong, “Camvox: A low-cost and accurate lidar-assisted visual slam system,” in 2021 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2021, pp. 5049–5055.
[26] S.-S. Huang, Z.-Y. Ma, T.-J. Mu, H. Fu, and S.-M. Hu, “Lidar-monocular visual odometry using point and line features,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 1091–1097.
[27] C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D. Tardós, “Orb-slam3: An accurate open-source library for visual, visual–inertial, and multimap slam,” IEEE Transactions on Robotics, vol. 37, no. 6, pp. 1874–1890, 2021.
[28] Y.-S. Shin, Y. S. Park, and A. Kim, “Dvl-slam: Sparse depth enhanced direct visual-lidar slam,” Autonomous Robots, vol. 44, no. 2, pp. 115–130, 2020.
[29] X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “Lic-fusion: Lidar-inertial-camera odometry,” in 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019, pp. 5848–5854.
[30] K. Sun, K. Mohta, B. Pfrommer, M. Watterson, S. Liu, Y. Mulgaonkar, C. J. Taylor, and V. Kumar, “Robust stereo visual inertial odometry for fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 3, no. 2, pp. 965–972, 2018.
[31] X. Zuo, Y. Yang, P. Geneva, J. Lv, Y. Liu, G. Huang, and M. Pollefeys, “Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking,” in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 5112–5119.
[32] D. Wisth, M. Camurri, S. Das, and M. Fallon, “Unified multi-modal landmark tracking for tightly coupled lidar-visual-inertial odometry,” IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1004–1011, 2021.
[33] J. Lin, C. Zheng, W. Xu, and F. Zhang, “R2live: A robust, real-time, lidar-inertial-visual tightly-coupled state estimator and mapping,” IEEE Robotics and Automation Letters, vol. 6, no. 4, pp. 7469–7476, 2021.
[34] C. F. W. Bell B.M., “The iterated kalman filter update as a gauss-newton method,” Automatic Control IEEE Transactions, vol. 38, no. 2, pp. 294–297, 1993.
[35] T. Shan, B. Englot, C. Ratti, and D. Rus, “Lvi-sam: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping,” in 2021 IEEE international conference on robotics and automation (ICRA). IEEE, 2021, pp. 5692–5698.
[36] J. Lin and F. Zhang, “R 3 live: A robust, real-time, rgb-colored, lidar-inertial-visual tightly-coupled state estimation and mapping package,” in 2022 International Conference on Robotics and Automation (ICRA). IEEE, 2022, pp. 10 672–10 678.
[37] ——, “R 3 live++: A robust, real-time, radiance reconstruction package with a tightly-coupled lidar-inertial-visual state estimator,” arXiv preprint arXiv:2209.03666, 2022.
[38] J. Engel, V. Usenko, and D. Cremers, “A photometrically calibrated benchmark for monocular visual odometry,” arXiv preprint arXiv:1607.02555, 2016.
[39] W. Wang, J. Liu, C. Wang, B. Luo, and C. Zhang, “Dv-loam: Direct visual lidar odometry and mapping,” Remote Sensing, vol. 13, no. 16, p. 3340, 2021.
[40] Z. Yuan, Q. Wang, K. Cheng, T. Hao, and X. Yang, “Sdv-loam: Semi-direct visual-lidar odometry and mapping,” IEEE Transactions on Pattern Analysis and Machine Intelligence, 2023.
[41] H. Zhang, L. Du, S. Bao, J. Yuan, and S. Ma, “Lvio-fusion:tightly-coupled lidar-visual-inertial odometry and mapping in degenerate environments,” IEEE Robotics and Automation Letters, vol. 9, no. 4, pp. 3783–3790, 2024.
[42] W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry package by tightly-coupled iterated kalman filter,” IEEE Robotics and Automation Letters, pp. 1–1, 2021.
[43] D. He, W. Xu, and F. Zhang, “Symbolic representation and toolkit development of iterated error-state extended kalman filters on manifolds,” IEEE Transactions on Industrial Electronics, 2023.
[44] D. Willner, C.-B. Chang, and K.-P. Dunn, “Kalman filter algorithms for a multi-sensor system,” in 1976 IEEE conference on decision and control including the 15th symposium on adaptive processes. IEEE, 1976, pp. 570–574.
[45] J. Ma and S. Sun, “Globally optimal distributed and sequential state fusion filters for multi-sensor systems with correlated noises,” Information Fusion, p. 101885, 2023.
[46] Y. Ren, Y. Cai, F. Zhu, S. Liang, and F. Zhang, “Rog-map: An efficient robocentric occupancy grid map for large-scene and high-resolution lidar-based motion planning,” arXiv preprint arXiv:2302.14819, 2023.
[47] R. M. Stereopsis, “Accurate, dense, and robust multiview stereopsis,” IEEE TRANSACTIONS ON PATTERN ANALYSIS AND MACHINE INTELLIGENCE, vol. 32, no. 8, 2010.
[48] S. Baker and I. Matthews, “Lucas-kanade 20 years on: A unifying framework,” International journal of computer vision, vol. 56, pp. 221–255, 2004.
[49] T.-M. Nguyen, S. Yuan, M. Cao, Y. Lyu, T. H. Nguyen, and L. Xie, “Ntu viral: A visual-inertial-ranging-lidar dataset, from an aerial vehicle viewpoint,” The International Journal of Robotics Research, vol. 41, no. 3, pp. 270–280, 2022.
[50] M. Helmberger, K. Morin, B. Berner, N. Kumar, G. Cioffi, and D. Scaramuzza, “The hilti slam challenge dataset,” IEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 7518–7525, 2022.
[51] L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scaramuzza, and M. Fallon, “Hilti-oxford dataset: A millimeter-accurate benchmark for simultaneous localization and mapping,” IEEE Robotics and Automation Letters, vol. 8, no. 1, pp. 408–415, 2022.
[52] H. Li, Y. Zou, N. Chen, J. Lin, X. Liu, W. Xu, C. Zheng, R. Li, D. He, F. Kong, et al., “Mars-lvig dataset: A multi-sensor aerial robots slam dataset for lidar-visual-inertial-gnss fusion,” The International Journal of Robotics Research, p. 02783649241227968, 2024.
[53] “Supplementary material: Fast-livo2: Fast, direct lidar-inertial-visual odometry,” available online: https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2˙supplementary.pdf.
[54] C. Klug, C. Arth, D. Schmalstieg, and T. Gloor, “Measurement uncertainty analysis of a robotic total station simulation,” in IECON 2018-44th Annual Conference of the IEEE Industrial Electronics Society. IEEE, 2018, pp. 2576–2582.
[55] Y. Ren, F. Zhu, W. Liu, Z. Wang, Y. Lin, F. Gao, and F. Zhang, “Bubble planner: Planning high-speed smooth quadrotor trajectories using receding corridors,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022, pp. 6332–6339.
[56] G. Lu, W. Xu, and F. Zhang, “On-manifold model predictive control for trajectory tracking on robotic systems,” IEEE Transactions on Industrial Electronics, vol. 70, no. 9, pp. 9192–9202, 2022.
[57] I. Vizzo, T. Guadagnino, J. Behley, and C. Stachniss, “Vdbfusion: Flexible and efficient tsdf integration of range sensor data,” Sensors, vol. 22, no. 3, 2022. [Online]. Available: https://www.mdpi.com/1424-8220/22/3/1296
[58] D. Cernea, “Openmvs: multi-view stereo reconstruction library. 2020,” URL: https://cdcseacave. github. io/openMVS, vol. 5, no. 6, p. 7, 2020.
[59] J. L. Schönberger and J.-M. Frahm, “Structure-from-motion revisited,” in 2016 IEEE Conference on Computer Vision and Pattern Recognition (CVPR), 2016, pp. 4104–4113.

FAST-LIVO2 补充材料：快速、直接的 LiDAR-惯性-视觉里程计

I. 系统模块验证

在本节中，我们验证了我们系统的关键模块，包括仿射扭曲、法线细化、参考补丁更新、按需光线投射查询、曝光时间估计以及利用 FAST-LIVO2 私有数据集和 MARS-LVIG 数据集的 ESIKF 顺序更新。

A. 仿射扭曲评估

在此实验中，我们旨在全面评估基于恒定深度假设（半密集方法中常用的技术）、来自点云的平面先验以及我们提出的系统（分别表示为“恒定深度”、“平面先验”和“平面法线细化”）的各种仿射扭曲效应。为实现这一目标，我们比较了三种方法在“CBD 大厦 02”和“办公楼墙壁”上的映射结果和漂移指标。如图 S2 所示，“平面法线精炼”提供了最清晰、最准确的映射结果，其次是“平面先验”。值得注意的是，“平面法线精炼”以显著的清晰度呈现了地面和墙壁上的文字和图案，以及车道标记。此外，“平面先验”和“平面法线精炼”相关的漂移保持在 $\displaystyle 0.01\text{\,}\mathrm{m}$ 以下，而“恒定深度”没有返回到起始位置，出现了 $\displaystyle 0.22\text{\,}\mathrm{m}$ 的漂移。这些结果证实了基于平面先验的仿射扭曲和通过平面法线精炼的增强的性能。

此外，我们比较了“恒定深度”和“平面先验”在“CBD 大厦 02”和“办公楼墙壁”序列上的扭曲投影效果。我们从这两个序列中随机选择了几个图像帧进行定性分析。对于每个帧，我们将附着在帧中可见的视觉地图点上的参考块投影到当前帧的空白图像上。此过程会生成一个新的 RGB 图像。如果仿射扭曲和姿态估计都执行良好，则具有块投影的区域将呈现无缝且最小失真外观，与原始 RGB 图像非常相似。图 S1 展示了扭曲块的比较。结果表明，“平面先验”下的姿态精度和扭曲性能明显优于“恒定深度”下的姿态精度和扭曲性能。

B. 参考面片更新和法线收敛的评估

在此实验中，我们验证了参考补丁更新策略和正常收敛对“HIT涂鸦墙”和“港大百年花园”的影响。如图 S3 所示，(a) 和 (b) 是这两个序列的重建点云。在右侧，对于从 A 到 H 的每个区域，我们分别展示了在不同姿态下捕获的五个补丁观察结果，每个补丁的大小为 $\displaystyle 40\times 40$ 像素，用于可视化。这些补丁是在位于左侧对应数字的相机帧中观察到的。可以看出，我们的参考路径更新策略倾向于选择一个高分辨率的参考补丁，该补丁沿着其法线方向面对平面。还注意到，尽管在非平面位置（例如，树叶、树干和路灯）生成了视觉地图点和补丁，但总体映射质量仍然很高。

我们还评估了我们提出的法线估计在 A 到 H 区域的补丁上的收敛性。每个补丁的大小为 $\displaystyle 11\times 11$ 像素。初始法向量是根据 LiDAR 点估计的。收敛曲线，表示在每次迭代次数时初始法向量和优化法向量之间的角度变化，如图 S3 所示。区域 A、C、D 和 E 是结构化区域，而区域 B、F、G 和 H 是非结构化区域。可以观察到，结构化区域的法向量收敛速度更快（在 6 次迭代内），法线细化较小（2 到 4 度），因为点云提供的初始法线比较准确。在非结构化区域，如灌木和树叶（即 B 和 F），法线细化非常显著（高达 9 度），需要 9 次迭代才能收敛。总体而言，在这 8 个区域的法线细化展示了良好的收敛特性。

C. 按需射线投射的评估

在此实验中，我们评估了按需射线投射模块在极端条件下的性能，在这些条件下，由于 LiDAR 的近距离盲区，当前和最近的 LiDAR 扫描只有很少甚至没有点 ¹¹1https://www.livoxtech.com/avia/specs。我们使用序列“狭窄走廊”进行深入分析，如图 S4 所示。在此序列中，我们穿过一条极其狭窄的隧道，其宽度约为 $\displaystyle 1.9\text{\,}\mathrm{m}$ ，并转向面对一侧的纹理较弱的墙壁。由于面向墙壁时 LiDAR 扫描中的点有限，我们只能通过体素查询获得少量视觉地图点（图 S4 (b) 中的黄色点）。在这种情况下，射线投射提供了足够的视觉约束来缓解退化（图 S4 (b) 中的蓝色点）。可视化结果表明，按需射线投射模块在 LiDAR 扫描中点数很少的具有挑战性的条件下运行良好。

D. 曝光时间估计评估

在此实验中，我们从两个方面验证了曝光时间估计模块： 1) 对于曝光时间和增益固定的序列，我们将接收到的原始图像的每个像素乘以一个随时间正弦变化的曝光因子。我们通过将估计的曝光时间与应用的正弦函数进行比较来验证估计的有效性。 2) 对于具有自动曝光和固定或自动增益设置的序列，我们通过将估计的曝光时间与从相机 API 中检索到的真实值进行比较来评估估计的曝光时间的准确性。

在第一部分中，我们在“零售街”序列上进行测试，将曝光因子应用于具有固定曝光时间和增益的图像。如图 S5 所示，估计的相对逆曝光时间与真实值非常吻合，证明了我们的曝光估计在合成条件下的收敛性。在第二部分中，我们使用“香港大学百年花园”、“香港大学文化花园”和“香港大学主楼”序列进行测试，这些序列具有显着的曝光时间变化。我们将估计的相对曝光时间按第一帧进行缩放，以恢复每一帧的实际曝光时间 (ms)。如图 S5 所示，估计的曝光时间与真实值非常吻合，验证了我们的曝光时间估计模块的有效性。偶尔的失配可能是由于未建模的响应函数和渐晕因子 [38] 造成的。

E. ESIKF 顺序更新评估

在此实验中，我们评估了 LiDAR 和相机状态的不同 ESIKF 更新策略。我们比较了异步和同步更新，以及标准更新和顺序更新。具体来说，我们评估了三种策略：“异步（标准更新）”，其中相机和激光雷达状态在各自的采样时间更新，不进行扫描重组；“同步（标准更新）”，其中激光雷达扫描被重组以与相机图像同步，状态用激光雷达和相机测量值在标准 ESIKF 中更新；以及“同步（顺序更新）”，其中激光雷达和相机同步，但状态首先用激光雷达测量值更新，然后用相机测量值更新。这些策略在准确性、鲁棒性和效率方面使用 MARS-LVIG 数据集的“AMvalley03”序列进行评估。我们选择此序列出于以下几个关键原因：

(1)

此序列包括导致激光雷达和视觉退化的斜坡，使其成为一个具有挑战性的测试案例。
(2)

此序列代表一个极大规模的场景（约 $\displaystyle 901\text{\,}\mathrm{m}$ $\displaystyle\times$ $\displaystyle 500\text{\,}\mathrm{m}$ $\displaystyle\times$ $\displaystyle 130\text{\,}\mathrm{m}$ ），具有长期和高速数据收集（覆盖 $\displaystyle 600\text{\,}\mathrm{s}$ ，速度为 $\displaystyle 12\text{\,}\mathrm{m}\mathrm{/}\mathrm{s}$ ），其中姿态偏差容易发生（由于长期和高速条件），即使轻微的漂移也会导致彩色点云的显著模糊（由于大规模），导致更明显的比较结果。
(3)

此序列提供了 RTK 真值数据，允许进行更准确的定量比较。

我们比较了三种更新策略的定性映射结果、定量 APE 和平均处理时间。实验配置如下：激光雷达更新涉及最多 5 次迭代，视觉更新使用三级金字塔，每级最多 5 次迭代，当相机和激光雷达在标准 ESIKF 中同时更新时，每级最多 3 次迭代，比例归一化因子（从视觉光度误差到激光雷达点到平面距离）对于“同步（标准更新）”设置为 0.0032，经过精心调整以获得最佳性能。

图 S6，(a-c) 展示了此序列的重建彩色点云。很明显，“同步（顺序更新）”策略产生了准确的映射结果，特别是在蓝色和橙色框突出显示的区域，其中山路重建没有任何分层。相反，其他两种策略在这些区域表现出错位，尽管“同步（标准更新）”的性能略好于“异步（标准更新）”。 “同步（顺序更新）”策略的优越性能主要归因于其在处理重大激光雷达和视觉退化方面的鲁棒性，如白色框 (c3) 中所示。该区域特征为一个大型无纹理斜坡，无人机以高速飞越，高度依赖于强先验。其他两种方法仅依赖于IMU先验，难以计算出相对准确的图像梯度下降方向，导致明显的线性化误差。

“AMvalley03”序列的APE(RMSE)指标分别为 $\displaystyle 3.12\text{\,}\mathrm{m}$ 、 $\displaystyle 2.45\text{\,}\mathrm{m}$ 和 $\displaystyle 0.68\text{\,}\mathrm{m}$ ，分别对应于“异步（标准更新）”、“同步（标准更新）”和“同步（顺序更新）”。在台式电脑（第IX-A节）上的平均处理时间约为 $\displaystyle 27.6\text{\,}\mathrm{ms}$ 、 $\displaystyle 49.9\text{\,}\mathrm{ms}$ 和 $\displaystyle 23.1\text{\,}\mathrm{ms}$ 。我们提出的“同步（顺序更新）”方法实现了最高的效率和精度，而“异步（标准更新）”方法的精度最低。 “同步（标准更新）”是最耗时的，主要是因为它需要在图像金字塔的每个级别融合所有激光雷达测量值。

总体而言，我们提出的“同步（顺序更新）”方法具有更高的精度和效率，而“异步（标准更新）”方法的精度最低，“同步（标准更新）”方法是最耗时的。

二、附加信息

表S1： FAST-LIVO2私有数据集概述

[b] Sequence Duration (minute:second) Sensor Degeneration Return Origin¹ Exposure / Gain Mode Exposure Time² Close Surface Distance³ Scene Characteristics Retail Street 2 min : 15 sec — ✓ Fixed / Fixed Outdoor CBD Building 01 1 min : 58 sec — ✓ Fixed / Auto Outdoor CBD Building 02 3 min : 54 sec LiDAR ✓ Auto / Fixed ✓ Outdoor CBD Building 03 5 min : 01 sec Camera, LiDAR ✓ Auto / Fixed ✓ Outdoor, Textureless HKU Landmark 1 min : 31 sec — ✓ Auto / Fixed ✓ Outdoor HKU Lecture Center 1 min : 17 sec LiDAR ✓ Auto / Fixed ✓ Indoor HKU Centennial Garden 1 min : 32 sec LiDAR ✓ Auto / Fixed ✓ Outdoor HKU Cultural Center 4 min : 01 sec LiDAR ✓ Auto / Fixed ✓ Outdoor HKU Main Building 1 min : 36 sec Camera, LiDAR ✓ Auto / Auto ✓ Out(In)door⁴, Textureless HKUST Red Sculpture 2 min : 10 sec — ✓ Fixed / Auto Outdoor, Cluttered HIT Graffiti Wall 8 min : 57 sec LiDAR ✓ Fixed / Auto Outdoor Banner Wall 1 min : 40 sec LiDAR ✓ Fixed / Auto Indoor, Dim Bright Screen Wall 1 min : 18 sec LiDAR ✓ Fixed / Auto Indoor Black Screen Wall 0 min : 56 sec Camera, LiDAR ✓ Fixed / Auto Indoor, Textureless Office Building Wall 1 min : 9 sec LiDAR ✓ Fixed / Auto ✓ Outdoor Narrow Corridor 1 min : 1 sec Camera, LiDAR ✓ Fixed / Auto ✓ Indoor, Dim, Textureless Long Corridor 1 min : 35 sec LiDAR ✓ Fixed / Auto ✓ Indoor, Dim Mining Tunnel 15 min : 49 sec Camera, LiDAR ✓ Fixed / Auto Indoor, Dim, Textureless SYSU 01 4 min : 40 sec — ✓ Auto / Auto Outdoor, Light⁵ SYSU 02 4 min : 30 sec Camera, LiDAR ✓ Auto / Auto Out(In)door, Light Total 66 min : 50 sec Camera, LiDAR ✓ Auto (Fixed) / Auto (Fixed) ✓ ✓ Out(In)door, Light, Dim, Textureless

1

序列通过循环飞行收集，从同一位置开始和结束。
2

从摄像头的API读取具有真实相机曝光时间的序列。
3

在某些序列中，由于近距离盲区，LiDAR 无法捕获或只能捕获有限的点云。
4

这些序列包括从室内环境到室外环境的移动过程。
5

这些序列的特点是存在显著的照明变化。

附表 S2：无人机自主导航实验概述

[b] Experiment Flight Duration (minute:second) Flight Mode⁶ Sensor Degeneration Return origin Exposure / Gain Mode Exposure Time Close Proximity to Obstacles Scene Characteristics Basement 5 min Autonomous Camera, LiDAR ✓ Auto / Auto ✓ Out(In)door, Light Narrow Opening 2 min:32 sec Manual Camera, LiDAR ✓ Auto / Auto ✓ ✓ Out(In)door, Light SYSU Campus 3 min:6 sec Manual — ✓ Auto / Auto Outdoor, Light Woods 2 min:49 sec Autonomous Camera ✓ Auto / Fixed ✓ Outdoor, Cluttered

6

在自主模式下，FAST-LIVO2、规划和 MPC 模块都在运行。在手动模式下，FAST-LIVO2 和 MPC 正在运行，而规划模块已禁用。

附表 S3： FAST-LIO2 在不同数据集上的每帧平均处理时间

[b] Hilti’22 & Hilti’23 Processing Time (ms) NTU VIRAL Processing Time (ms) Private Dataset Processing Time (ms) Construction Ground 24.32 eee_01 17.63 Retail Street 10.23 Construction Multilevel 23.21 eee_02 17.33 CBD Building 01 10.40 Construction Stairs $\displaystyle\times$ eee_03 15.32 CBD Building 02 $\displaystyle\times$ Long Corridor 28.43 nya_01 19.99 CBD Building 03 $\displaystyle\times$ Cupola $\displaystyle\times$ nya_02 18.62 HKU Landmark 11.62 Lower Gallery 24.49 nya_03 19.32 HKU Lecture Center $\displaystyle\times$ Attic to Upper Gallery $\displaystyle\times$ nya_04 15.71 HKU Centennial Garden $\displaystyle\times$ Outside Building 18.67 sbs_02 16.10 HKU Cultural Center $\displaystyle\times$ Floor 0 27.34 sbs_03 17.71 HKU Main Building $\displaystyle\times$ Floor 1 27.12 HKUST Red Sculpture 10.98 Floor 2 25.22 HIT Graffiti Wall $\displaystyle\times$ Basement 22.31 Banner Wall $\displaystyle\times$ Stairs 20.91 Bright Screen Wall $\displaystyle\times$ Parking 3x floors down $\displaystyle\times$ Black Screen Wall $\displaystyle\times$ Large room 29.26 Office Building Wall $\displaystyle\times$ Large room (dark) 28.20 Narrow Corridor $\displaystyle\times$ Long Corridor $\displaystyle\times$ Mining Tunnel $\displaystyle\times$ SYSU 01 11.21 SYSU 02 $\displaystyle\times$ Overall Average 19.68

7

$\displaystyle\times$ 表示系统完全失败。