FastLivo2论文翻译

andlbds

于 2025-02-25 15:35:57 发布

阅读量856

点赞数 10

文章标签：论文阅读

原文链接：https://arxiv.org/abs/2408.14035

版权

摘要

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

I. 引言

近年来，同时定位与建图（SLAM）技术取得了显著进展，尤其是在未知环境中的实时三维重建和定位方面。由于其能够在实时估计姿态的同时重建地图，SLAM已成为各种机器人导航任务中不可或缺的技术。定位过程为机器人的机载控制器提供了关键的状态反馈，而密集的三维地图则提供了关键的环境信息，例如自由空间和障碍物，这对于有效的轨迹规划至关重要。彩色地图还携带了大量的语义信息，能够生动地呈现真实世界，为虚拟现实、增强现实、三维建模和人机交互等应用开辟了广阔的可能性。

目前，已有多种SLAM框架成功地应用于单一传感器测量，主要是相机 [1]–[4] 或 激光雷达 [5]–[7]。尽管视觉SLAM和激光雷达SLAM在各自领域中表现出色，但它们各自存在固有的局限性，限制了它们在不同场景中的性能。视觉SLAM利用成本较低的CMOS传感器和镜头，能够建立准确的数据关联，从而实现一定程度的定位精度。丰富的颜色信息进一步丰富了语义感知能力。通过利用这种增强的场景理解能力，深度学习方法被用于鲁棒的特征提取和动态目标过滤。然而，视觉SLAM缺乏直接的深度测量，需要通过三角测量或深度滤波等操作同时优化地图点，这引入了显著的计算开销，通常限制了地图的精度和密度。视觉SLAM还面临许多其他限制，例如不同尺度下的测量噪声变化、对光照变化的敏感性以及无纹理环境对数据关联的影响。

激光雷达SLAM利用激光雷达传感器直接获得精确的深度测量，在定位和建图任务中相比视觉SLAM具有更高的精度和效率。然而，激光雷达SLAM也存在一些显著的不足。一方面，它重建的点云地图虽然详细，但缺乏颜色信息，从而降低了其信息量。另一方面，激光雷达SLAM在缺乏几何约束的环境中（例如狭窄的隧道、单一且延伸的墙壁等）表现会恶化。

随着智能机器人在真实世界中的应用需求不断增长，尤其是在那些通常缺乏结构或纹理的环境中，仅依赖单一传感器的现有系统无法提供所需的准确且鲁棒的姿态估计。为了解决这一问题，融合常用传感器（如激光雷达、相机和IMU）的策略越来越受到关注。这种策略不仅结合了这些传感器的优势以提供更准确的姿态估计，还能够构建准确、密集且彩色的点云地图，即使在单个传感器性能退化的环境中也能实现。高效且准确的激光雷达-惯性-视觉里程计（LIVO）和建图仍然是一个具有挑战性的问题：

整个LIVO系统需要处理激光雷达测量数据（每秒数百到数千个点）以及高分辨率、高帧率的图像。充分利用如此大量的数据，尤其是在有限的机载资源下，需要卓越的计算效率。
许多现有系统通常包含一个激光雷达-惯性里程计（LIO）子系统和一个视觉-惯性里程计（VIO）子系统，每个子系统都需要从视觉和激光雷达数据中分别提取特征以降低计算负担。在缺乏结构或纹理的环境中，这种提取过程往往导致特征点有限。此外，为了优化特征提取，需要针对激光雷达扫描模式和点密度的变化进行广泛的工程适配。
为了降低计算需求并实现相机和激光雷达测量之间的紧密集成，需要一个统一的地图来同时管理稀疏点和高分辨率图像测量。然而，设计和维护这种地图特别具有挑战性，考虑到激光雷达和相机测量的异构性。
为了确保重建的彩色点云的准确性，姿态估计需要达到像素级精度。达到这一标准面临相当大的挑战：适当的硬件同步、激光雷达和相机之间外参的严格预校准、精确恢复曝光时间，以及能够在实时达到像素级精度的融合策略。

鉴于这些问题，我们提出了FAST-LIVO2，这是一个高效率的LIVO系统，通过顺序更新的误差状态迭代卡尔曼滤波器（ESIKF）紧密地融合激光雷达、图像和IMU测量数据。借助IMU传播的先验信息，系统状态首先通过激光雷达测量数据进行更新，然后通过图像测量数据进行更新，两者均基于直接法和单一体素地图。具体来说，在激光雷达更新中，系统将原始点注册到地图中以构建和更新其几何结构；在视觉更新中，系统直接将激光雷达地图点用作视觉地图点，无需从图像中提取、三角测量或优化任何视觉特征。地图中选定的视觉地图点附带了之前观察到的参考图像块，然后将其投影到当前图像中，通过最小化直接光度误差来对齐其姿态（即稀疏图像对齐）。为了提高图像对齐的准确性，FAST-LIVO2动态更新参考块，并利用从激光雷达点获得的平面先验。为了提高计算效率，FAST-LIVO2使用激光雷达点来识别当前图像中可见的视觉地图点，并在没有激光雷达点的情况下按需进行体素射线投射。FAST-LIVO2还实时估计曝光时间以处理环境光照变化。

FAST-LIVO2是基于我们之前提出的FAST-LIVO [8]开发的。与FAST-LIVO相比，FAST-LIVO2的新贡献如下：

我们提出了一个高效的ESIKF框架，采用顺序更新来解决激光雷达和视觉测量数据之间的维度不匹配问题，提高了FAST-LIVO的鲁棒性，FAST-LIVO使用异步更新。
我们使用（甚至细化）激光雷达点的平面先验来提高准确性。相比之下，FAST-LIVO假设一个块中的所有像素共享相同的深度，这是一个过于简化的假设，显著降低了图像对齐中仿射变换的准确性。
我们提出了一个参考块更新策略，通过选择高质量、内点的参考块来提高图像对齐的准确性，这些参考块具有较大的视差和足够的纹理细节。FAST-LIVO根据与当前视图的接近程度选择参考块，通常导致低质量的参考块，从而降低准确性。
我们进行了在线曝光时间估计，以处理环境光照变化。FAST-LIVO没有解决这一问题，导致在光照变化显著时图像对齐收敛性差。
我们提出了按需体素射线投射，以增强系统在激光雷达点测量缺失时的鲁棒性，这是由于激光雷达近距离盲区导致的，FAST-LIVO没有考虑这一问题。

上述每项贡献均通过全面的消融研究进行了验证，以证明其有效性。我们将提出的系统实现为实用的开源软件，针对英特尔和ARM处理器进行了精心优化，以实现实时运行。该系统具有广泛的适用性，支持多线旋转激光雷达、新兴的固态激光雷达（具有非常规扫描模式），以及针孔相机和各种鱼眼相机。此外，我们在25个公共数据集序列（即Hilti和NTU-VIRAL数据集）以及各种代表性私有数据集上进行了广泛的实验，使我们能够与其他最先进的SLAM系统（例如R3LIVE、LVI-SAM、FAST-LIO2等）进行比较。定性和定量结果表明，我们提出的系统在准确性和鲁棒性方面显著优于其他同类系统，且计算成本更低。

为了进一步突出我们系统的实际适用性和多功能性，我们在三个不同的应用中进行了部署。首先，完全机载的自主无人机导航，展示了系统的实时能力，这是首次将激光雷达-惯性-视觉系统应用于实际的自主无人机飞行。其次，航空测绘展示了系统在无结构环境下的像素级精度。最后，高质量的网格、纹理和基于神经辐射场（NeRF）模型的生成进一步证明了系统适用于渲染任务。我们在GitHub上提供了代码和数据集。

II. 相关工作

A. 直接法

直接法是视觉和激光雷达SLAM中用于快速姿态估计的突出方法。与基于特征的方法（例如，从图像中提取角点和边缘像素；从激光雷达扫描中提取平面和边缘点）不同，直接法直接利用原始测量数据，通过最小化基于光度误差或点到平面残差的误差函数来优化传感器姿态 [11]。通过省略耗时的特征提取和匹配步骤，直接法能够快速估计姿态。然而，缺乏特征匹配需要相当准确的状态先验估计，以避免陷入局部最小值。

视觉SLAM中的直接方法大致可以分为密集直接法、半密集直接法和稀疏直接法。密集直接法主要用于配备完整深度测量的RGB-D相机，例如 [15]–[17]，通过图像到模型的对齐来估计姿态。相比之下，半密集直接法 [3, 18] 通过利用具有显著灰度梯度的像素进行估计，实现直接图像对齐。与密集和半密集直接方法相比，稀疏直接法 [2, 12] 专注于通过仅使用少数几个精心选择的原始块来提供准确的状态估计，从而进一步减少了计算负担。

与直接视觉SLAM方法不同，直接激光雷达SLAM系统 [13, 14, 19, 20] 不区分密集和稀疏方法，通常使用每个扫描中的空间下采样或时间下采样的原始点来构建姿态优化的约束。在我们的工作中，我们为激光雷达和视觉模块都采用了直接法的原则。我们的激光雷达模块基于VoxelMap [14]，视觉模块基于稀疏直接法 [12] 的变体。尽管我们的视觉模块受到 [12] 中稀疏直接图像对齐的启发，但它通过将激光雷达点重新用作视觉地图点，从而减少了后端计算的强度（例如，特征对齐、滑动窗口优化和/或深度滤波）。

B. 激光雷达-视觉（惯性）SLAM

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

Zhang等人提出了一种激光雷达-视觉-惯性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的高级版本R3LIVE++ [37] 实时估计曝光时间，并提前进行光度校准 [38]，这使得系统能够恢复地图点的辐射亮度。与大多数上述依赖于特征的方法的激光雷达-惯性-视觉系统不同，R3LIVE系列 [36, 37] 在LIO和VIO子系统中都采用了直接方法，无需特征提取，使它们能够在纹理较少或结构较少的场景中捕捉到微妙的环境特征。

我们的系统也联合使用激光雷达、图像和IMU数据来估计状态，并在测量层面维护一个紧耦合的体素地图。此外，我们的系统使用直接方法，利用原始激光雷达点进行激光雷达扫描配准，并使用原始图像块进行视觉跟踪。我们的系统与R3LIVE（或R3LIVE++）的关键区别在于，R3LIVE（和R3LIVE++）在VIO中在单个像素层面运行，而我们的系统在图像块层面运行。这种差异使我们的系统具有显著的优势。首先，在鲁棒性方面，我们的方法使用简化的一步帧到地图稀疏图像对齐来进行姿态估计，减少了对准确初始状态的依赖，而R3LIVE需要通过帧到帧的光流获得准确的初始状态。因此，我们的系统简化并改进了R3LIVE中的两阶段帧到帧和帧到地图操作。其次，从计算角度来看，R3LIVE的VIO主要采用密集直接方法，计算成本较高，需要大量的点来构建残差和渲染。相比之下，我们的稀疏直接方法提供了更高的计算效率。最后，我们的系统利用原始图像块层面的信息，而R3LIVE的分辨率受限于其点地图。我们的视觉模块与DVLOAM [39]、SDV-LOAM [40] 和LVIO-Fusion [41] 最为相似，它们将带有块的激光雷达点投影到新图像中，并通过最小化直接光度误差来跟踪图像。然而，它们存在几个关键差异，例如使用单独的地图用于视觉和激光雷达、在视觉模块中假设块变形中的恒定深度、在状态估计层面松耦合以及视觉对齐的两阶段帧到帧和帧到关键帧。相比之下，我们的系统在迭代卡尔曼滤波器中紧密集成了帧到地图图像对齐、激光雷达扫描配准和IMU测量。此外，得益于激光雷达和视觉模块的单一体素地图，我们的系统可以直接利用激光雷达点提供的平面先验来加速图像对齐。

III. 系统概述

本节介绍了FAST-LIVO2的总体架构，如图2所示，系统主要分为四个部分：误差状态迭代卡尔曼滤波器（ESIKF）、局部地图、激光雷达测量模型和视觉测量模型。以下将分别对这四个部分进行详细说明。

1. 误差状态迭代卡尔曼滤波器（ESIKF）

FAST-LIVO2采用误差状态迭代卡尔曼滤波器（ESIKF）作为核心的状态估计器，用于融合激光雷达、视觉和惯性测量数据。ESIKF通过顺序更新策略，解决了激光雷达和视觉测量数据之间的维度不匹配问题，提高了系统的鲁棒性和效率。

状态定义：系统状态包括IMU的姿态、位置、速度、偏置以及相机的曝光时间等。
传播：在两个连续的激光雷达扫描之间，利用IMU测量数据进行状态的前向传播。
更新：通过激光雷达和视觉测量数据进行状态的后向更新，采用顺序更新策略，先进行激光雷达更新，再进行视觉更新。

2. 局部地图

局部地图采用自适应体素结构，用于存储和管理激光雷达点和视觉地图点。

体素结构：体素结构通过哈希表和八叉树组织，能够高效地存储和查询激光雷达点。
几何结构构建：通过激光雷达点构建地图的几何结构，包括平面的检测和参数估计。
视觉地图点生成：从当前帧中选择具有显著灰度梯度的点作为视觉地图点，并将其与激光雷达点关联。

3. 激光雷达测量模型

激光雷达测量模型用于计算激光雷达点到地图平面的残差，以进行状态更新。

点到平面残差：计算激光雷达点在地图中的位置与平面模型之间的残差。
测量噪声建模：考虑激光雷达点的测距和方位角噪声，以及激光束发散角的影响。

4. 视觉测量模型

视觉测量模型用于计算视觉地图点在当前帧中的光度误差，以进行状态更新。

视觉地图点选择：通过可见体素查询和按需射线投射，选择当前帧中可见的视觉地图点。
光度误差计算：计算视觉地图点在当前帧中的光度误差，采用稀疏直接方法进行图像对齐。
异常点剔除：剔除被遮挡或深度不连续的视觉地图点，以提高图像对齐的准确性。

通过上述四个部分的紧密协作，FAST-LIVO2能够在复杂环境中实现高精度、高鲁棒性的状态估计和地图构建。

IV. 误差状态迭代卡尔曼滤波器与顺序状态更新

本节详细介绍了基于顺序更新的误差状态迭代卡尔曼滤波器（ESIKF）框架。该框架通过顺序更新策略，解决了激光雷达和视觉测量数据之间的维度不匹配问题，并提高了系统的鲁棒性和效率。

A. 符号和状态转移模型

在我们的系统中，我们假设三种传感器（激光雷达、IMU和相机）之间的时间偏移是已知的，可以提前校准或同步。我们将IMU框架（记作I）视为机体框架，将第一个机体框架视为全局框架（记作G）。此外，我们假设三种传感器是刚性连接的，外参（见表I）是预先校准的。

表 I：重要符号说明
符号	说明
⊞/⊟	状态流形上的“盒加”与“盒减”操作（封装形式）
G(⋅)	全局世界坐标系中的向量 (⋅)
C(⋅)	相机坐标系中的向量 (⋅)
$_{}^{I}\textrm{}T_L$	LiDAR 坐标系相对于 IMU 坐标系的外参（位姿变换矩阵）
$_{}^{C}\textrm{T}_I$	IMU 坐标系相对于相机坐标系的外参（位姿变换矩阵）
$_{}^{G}\textrm{T}_I$	全局坐标系下 IMU 坐标系在时刻 kk 的位姿（相对于全局坐标系）
$x, \hat x, \bar{x}$	x 的真值、预测值和更新后的估计值
$\tilde{x}_k$	xx的第 k 次迭代更新估计值
$\delta x$	x 的真值与估计值之间的误差状态

然后，在第i次IMU测量时，离散状态转移模型为：

$x_{i+1} = x_i \oplus (\Delta t f(x_i, u_i, w_i))$

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

$M \triangleq SO(3) \times \mathbb{R}^{16}, \quad \dim(M) = 19$

$x \triangleq \begin{bmatrix} GR_I^T & Gp_I^T & Gv_I^T & b_g^T & b_a^T & Gg^T & \tau \end{bmatrix}^T \in M$

$u \triangleq \begin{bmatrix} \omega_m^T & a_m^T \end{bmatrix}^T, \quad w \triangleq \begin{bmatrix} n_g^T & n_a^T & n_{bg}^T & n_{ba}^T & n_{\tau}^T \end{bmatrix}^T$

$f(x, u, w) = \begin{bmatrix} \omega_m - b_g - n_g \\ Gv_I + \frac{1}{2}(GR_I(a_m - b_a - n_a) + Gg)\Delta t \\ GR_I(a_m - b_a - n_a) + Gg \\ n_{bg} \\ n_{ba} \\ 0_{3 \times 1} \\ n_{\tau} \end{bmatrix}$

其中，GRI、GpI 和 GvI 分别表示IMU在全局框架中的姿态、位置和速度，Gg 是全局框架中的重力向量，τ 是相对于第一帧的相机曝光时间的倒数，nτ 是将 τ 视为随机游走的高斯噪声，ωm 和 am 是原始IMU测量值， $n_g$ 和 $n_a$ 是 $\omega _m$ 和 $a _m$ 中的测量噪声， $b _a$ 和 $b _g$ 是IMU偏差，分别由高斯噪声 $n _{bg}$ 和 $n _{ba}$ 驱动。

B. 扫描重组

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

C. 传播

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

D. 顺序更新

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

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

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

$\begin{bmatrix} y_l \\ y_c \end{bmatrix} = \begin{bmatrix} h_l(x, v_l) \\ h_c(x, v_c) \end{bmatrix}$

其中， $v_l \sim \mathcal{N}(0, \Sigma_{v_l})$ 和 $v_c \sim \mathcal{N}(0, \Sigma_{v_c})$ 分别表示激光雷达和相机的测量噪声。一个标准的ESIKF会使用所有当前测量数据（包括激光雷达测量 $y_l$ 和图像测量 $y_c$ ）来更新状态 x。然而，激光雷达和图像测量是两种不同的传感方式，它们的数据维度不匹配。此外，图像测量的融合可能在图像金字塔的不同层级进行。为了处理维度不匹配问题并为每个模块提供更多的灵活性，我们提出了顺序更新策略。假设激光雷达测量 $y_l$ 和图像测量 $y_c$ 在给定状态向量 x 的情况下是统计独立的（即，测量受到统计独立噪声的干扰），该策略在理论上等同于使用所有测量数据的标准更新。

为了引入顺序更新，我们将当前状态 x 的总条件分布重写如下：

方程（5）表明，总条件分布 $p(x|{y_l},{y_c})$ 可以通过两次顺序贝叶斯更新获得。第一步仅将激光雷达测量 $y_l$ 与IMU传播的先验分布 p(x) 融合，以获得分布 $p(x|{y_l})$ ：

$p(x|y_l) \propto p(y_l|x)p(x)$

第二步将相机测量 $y_c$ 与 $p(x|{y_l})$ 融合，以获得最终的后验分布 $p(x|{y_l},{y_c})$ ：

$p(x|y_l, y_c) \propto p(y_c|x)p(x|y_l)$

为了进行（8）中的融合，无论是激光雷达还是图像测量，我们详细说明先验分布 $q(x)$ 和测量模型 $q(y|x)$ 。对于先验分布 $q(x)$ ，记作 $x = \hat{x} \oplus \delta x$ ，其中 $\delta x \sim \mathcal{N}(0, \hat{P})$ 。在激光雷达更新（即第一步）的情况下， $(\hat{x}, \hat{P})$ 是传播步骤中的状态和协方差。在视觉更新（即第二步）的情况下， $(\hat{x}, \hat{P})$ 是激光雷达更新收敛后的状态和协方差。为了获得测量模型分布 $q(y|x)$ ，记在第 $\kappa$ 次迭代时估计的状态为 $\hat{x}^\kappa$ ，其中 $\hat{x}^0 = \hat{x}$ 。通过在 $\hat{x}^\kappa$ 处对测量模型（4）（无论是激光雷达还是相机测量）进行一阶泰勒展开，得到：

$y|x \approx h(\hat{x}^\kappa, 0) + H^\kappa \delta x^\kappa + L^\kappa v$

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

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

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

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

收敛后的状态和协方差矩阵随后成为后验分布 $q(x|y)$ 的均值和协方差。卡尔曼滤波器的顺序更新已在文献中进行了研究，例如在 [44, 45] 中。本文采用这种方法用于激光雷达和相机系统的ESIKF。算法1详细描述了ESIKF的顺序更新实现。在第一步（第6-10行），通过迭代更新误差状态，直到收敛，从而从激光雷达测量数据中获得更新（见第六节A）。收敛后的状态和协方差估计值，再次记作 $\hat x$ 和 $\hat P$ ，用于更新地图的几何结构（见第五节B），然后在第二步视觉更新（第13-23行）中在图像金字塔的每个层级上进行细化（见第七节B），直到收敛。最终的状态和协方差 xˉ 和 Pˉ 用于传播传入的IMU测量数据（见第四节C）并更新地图的视觉结构（见第五节D和E）。

V. 局部地图

A. 地图结构

我们的地图采用了自适应体素结构，该结构通过哈希表和每个哈希条目的八叉树来组织（如图2所示）。哈希表管理根体素，每个根体素的固定尺寸为0.5×0.5×0.5米。每个根体素包含一个八叉树结构，用于进一步组织不同大小的叶子体素。一个叶子体素代表一个局部平面，存储一个平面特征（即平面中心、法向量和不确定性）以及一组位于该平面上的激光雷达原始点。其中一些点附加了三级图像块（8×8像素大小），我们称这些点为视觉地图点。收敛的视觉地图点仅附加参考块，而非收敛的点则附加参考块和其他可见块（见第五节E）。叶子体素的可变大小使其能够表示不同尺度的局部平面，从而适应具有不同结构的环境 [14]。

为了防止地图大小无限制增长，我们仅保留一个局部地图，该地图围绕激光雷达当前位置的大型局部区域内（长度为L），如图4所示的二维示例。最初，地图是一个以激光雷达起始位置为中心的立方体。激光雷达的探测区域以激光雷达当前位置为中心的球体可视化，其半径由激光雷达的探测范围定义。当激光雷达移动到新位置 p1 时，探测区域触及地图边界，我们将地图沿边界移动一段距离 d。随着地图的移动，移出局部地图区域的内存空间将被重置，以存储新移入局部地图的区域。这种环形缓冲区方法确保我们的局部地图在固定大小的内存中保持更新。环形缓冲区哈希图的实现细节在 [46] 中进行了详细描述。在每次ESIKF更新步骤后执行地图移动检查。

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

地图的几何结构是通过激光雷达点测量数据构建和更新的。具体来说，在ESIKF（第四节）的激光雷达更新之后，我们将激光雷达扫描中的所有点注册到全局框架中。对于每个注册的激光雷达点，我们确定其在哈希图中的根体素位置。如果该体素不存在，则用新点初始化该体素，并将其索引到哈希图中。如果确定的体素已经存在于地图中，则将该点附加到现有体素中。在扫描中的所有点分配完毕后，我们按照以下方式执行几何结构的构建和更新。对于新创建的体素，我们根据奇异值分解确定体素中包含的所有点是否位于一个平面上。如果是，我们计算平面的中心点 q=pˉ、法向量 n 以及平面的协方差矩阵 Σn,q（平面中心和法向量的协方差矩阵），用于表征平面的不确定性。这种不确定性来源于姿态估计的不确定性和点测量噪声。平面参数和不确定性的详细计算方法以及平面标准可以参考我们之前的工作 [14]。如果体素中的点不形成平面，则将体素连续细分，直到体素中的点被确定形成平面，或者达到最大层级（例如3层）。在后一种情况下，将丢弃叶子体素中的点。因此，地图中只包含被识别为平面的体素（无论是根体素还是子体素）。对于有新点附加到现有体素的情况，我们评估新点是否与根体素或子体素中的现有点形成平面。如果不是，我们按照上述方法进行体素细分。如果是，我们按照上述方法更新平面参数 (q,n) 和协方差 Σn,q。一旦平面参数收敛（见 [14]），该平面将被视为成熟，新点将被丢弃。此外，成熟平面上的激光雷达点将用于后续部分中视觉地图点的生成。对于成熟平面，最近的50个激光雷达点是视觉地图点生成的候选点，而对于未成熟的平面，所有激光雷达点都是候选点。视觉地图点生成过程将从这些候选点中识别出一些点作为视觉地图点，并将它们与当前图像块相关联，用于图像对齐。

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

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

D. 参考块的更新

由于一个视觉地图点可能有多个图像块，我们需要为图像对齐选择一个参考块。具体来说，我们根据光度相似性和视角对每个图像块 f 进行评分，如下所示：

$\text{NCC}(f, g) = \frac{\sum_{x,y} [f(x, y) - \bar{f}][g(x, y) - \bar{g}]}{\sqrt{\sum_{x,y} [f(x, y) - \bar{f}]^2 \sum_{x,y} [g(x, y) - \bar{g}]^2}}$

$c = \frac{n \cdot p}{\|p\|}, \quad \omega_1 = \frac{1}{1 + e^{\text{tr}(\Sigma_n)}}$

$S = (1 - \omega_1) \cdot \frac{1}{n} \sum_{i=1}^{n} \text{NCC}(f, g_i) + \omega_1 \cdot c$

其中， $\text{NCC}(f, g)$ 表示归一化交叉相关性，用于测量在参考块和目标块的最高分辨率层级（0级）的两个块之间的相似性，同时对两个块进行均值减法；c 表示图像块法向量 n 和视角方向 $\frac{ p}{\|p\|}$ 之间的余弦相似性。当图像块正对着平面时，c 的值为1。总分 S 通过将加权的 NCC 和 c 相加得到，其中前者表示图像块 f 与其他图像块 gi 之间的平均相似性， ${{\text{tr}(\Sigma_n)}}$ 表示法向量协方差矩阵的迹。在附加到视觉地图点的所有图像块中，得分最高的图像块被更新为参考块。上述评分机制倾向于选择参考块，这些参考块的外观（以 NCC 衡量）与大多数其他图像块相似，并且其视角与平面正交，从而在高分辨率下保持纹理细节。相比之下，我们之前的工作 FAST-LIVO [8] 以及其他先前的研究 [4] 直接选择与当前帧视角差异最小的图像块作为参考块，这导致所选参考块非常接近当前帧，从而对当前姿态更新施加了较弱的约束。

E. 法向量的细化

每个视觉地图点都被假设位于一个小的局部平面上。先前的研究 [2, 4, 8] 假设一个块中的所有像素具有相同的深度，这是一个过于简化的假设，通常不成立。我们使用从激光雷达点计算出的平面参数（见第五节B）来实现更高的精度。这个平面法向量对于视觉更新过程中图像对齐的仿射变换至关重要。为了进一步提高仿射变换的精度，可以从附加到视觉地图点的图像块中进一步细化平面法向量。具体来说，我们通过最小化参考块与其他图像块之间的光度误差来优化平面法向量。

VI. 激光雷达测量模型

本节详细介绍了用于ESIKF（第四节）中激光雷达更新的激光雷达测量模型 $y_l = h_l(x, v_l)$ 。

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

在获得扫描中的未失真点 ${\{Lpi}\}$ 后，我们使用在激光雷达更新的第 κ 次迭代中估计的状态 $\hat x^k$ ，将这些点投影到全局框架中：

$G\hat{p}_j^\kappa = G\hat{T}_I^T \cdot ITL \cdot Lp_j$

然后，我们确定 $G\hat{p}_j^\kappa$ 所在的哈希图中的根体素或子体素。如果未找到体素，或者体素中不包含平面，则丢弃该点。否则，我们使用体素中的平面来为激光雷达点建立测量方程。具体来说，我们假设在准确的激光雷达姿态 $GTI$ 下，真实的激光雷达点 $Lp^{gt}_j$ 应该位于体素中的平面上，该平面的法向量为 $n^{gt}_j$ ，中心点为 $q^j_{gt}$ ，即：

$0 = (n_j^{gt})^T (GTI \cdot ITL \cdot Lp_j^{gt} - q_j^{gt})$

由于真实的激光雷达点 $Lp_j^{gt}$ 被测量为 $Lp_j$ ，且存在测距和方位角噪声 $\delta Lp_j$ ，因此有 $Lp_j^{gt} = Lp_j - \delta Lp_j$ 。同样，平面参数 $(n_j^{gt}, q_j^{gt})$ 被估计为 $(n_j, q_j)$ ，且存在协方差 $\Sigma_{n,q}$ （见第五节B），因此有 $n_j^{gt} = n_j \ominus \delta n_j$ 和 $q_j^{gt} = q_j - \delta q_j$ 。因此，我们有：

$0 \approx y_l = (n_j \ominus \delta n_j)^T (GTI \cdot ITL \cdot (Lp_j - \delta Lp_j) - (q_j - \delta q_j)) \triangleq h_l(x, v_l)$

其中，测量噪声 $v_l = (\delta Lp_j, \delta n_j, \delta q_j)$ 包括与激光雷达点、法向量和平面中心相关的噪声。

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

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

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

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

VII. 视觉测量模型

本节详细介绍了用于ESIKF（第四节）中视觉更新的视觉测量模型 $y_c = h_c(x, v_c)$ 。

A. 视觉地图点的选择

为了在视觉更新中进行稀疏图像对齐，我们首先选择合适的视觉地图点。我们首先从地图中提取当前相机视场内可见的地图点子集（称为视觉子地图），然后剔除异常点。这一过程产生了一组经过筛选的视觉地图点，用于构建视觉光度误差的视觉测量模型。

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

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

上述提取的视觉地图点 {Gpi} 用于构建视觉测量模型。其基本原理是，当使用真实姿态（即，姿态）xk 将地图点 Gpi 转换到当前图像 Ik(⋅) 时，参考块与当前块之间的光度误差应为零：

$0 = \tau_k I_k^{gt} \left( \pi \left( C_I^T \left( G_I^T \right)^{-1} G p_i \right) \triangleq u_i + \Delta u \right) - \tau_r I_r^{gt} \left( \pi \left( C_r^T G G p_i \right) \triangleq u'_i + A_r^i \Delta u \right)$

其中，π(⋅) 是常见的相机投影模型（例如，针孔、MEI、ATAN、Scaramuzza、等距）， $C_r^TG$ 是全局框架 G 相对于参考框架 Cr 的姿态，该姿态在接收和融合参考帧时已经估计完成， $A_r^i$ 是将第 i 个当前块的像素转换到参考块的仿射变换矩阵，Δu 是当前块内中心 ui 的相对像素位置， $I_k^{gt}$ 和 $I_r^{gt}$ 分别表示参考帧和当前帧的真实像素值。这些值被测量为实际图像像素值 $I_k$ 和 $I_r$ ，并存在测量噪声 $v_c =(\delta I_k, \delta I_r)$ ，这些噪声来源于各种因素（例如，相机CMOS的散粒噪声和模数转换器（ADC）噪声）。因此，我们有：

为了提高计算效率，我们采用了逆组合公式 [4, 48]，将参数化姿态增量 δT∈R6（见（21）中的 GTI=GT^Iexp(δT)）从 ui 移动到 ui′，如下所示：

$0 \approx y_c = \tau_k (I_k (u_i + \Delta u) - \delta I_k) - \tau_r (I_r (u'_i + A_r^i \Delta u) - \delta I_r) \triangleq h_c(x, v_c)$

鉴于 ui′ 在参考帧中保持不变，我们只需要对 δT 的雅可比矩阵进行一次计算，而无需在每次迭代中重新计算。为了从测量方程（22）中估计曝光时间 τk，我们将初始曝光时间 τ0 固定为1，以消除当所有曝光时间均为零时方程（22）出现的退化现象。随后各帧的估计曝光时间因此是相对于第一帧的曝光时间。方程（22）用于视觉更新步骤中的三个层级（见算法1）；在每个层级收敛后，进入下一个更精细的层级。估计的状态随后用于生成视觉地图点（见第五节C）和更新参考块（见第五节D）。

VIII. 用于评估的数据集

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

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

NTU-VIRAL 数据集是在南洋理工大学校园内使用航空平台收集的，涵盖了多样化的场景，体现了独特的航空操作挑战。具体来说，“sbs”序列只能从远处物体提供嘈杂的视觉特征。“nya”序列对激光雷达 SLAM 来说，由于半透明表面，对视觉 SLAM 来说，由于复杂的飞行动态和低光照条件，都存在挑战。该数据集配备了 16 通道 OS1 gen13 激光雷达，采样频率为 10 Hz，内置 IMU 采样频率为 100 Hz，以及两个同步的针孔相机，触发频率为 10 Hz。使用左相机进行评估。Hilti'22 和 Hilti'23 数据集由手持和机器人设备收集，涵盖了建筑工地、办公室、实验室和停车场等室内外环境的序列。这些序列引入了许多挑战，包括长走廊、地下室和楼梯，存在纹理缺失、光照变化以及激光雷达平面约束不足等问题。手持序列使用 Hesai PandarXT-324 激光雷达，采样频率为 10 Hz，五台广角相机，采样频率为 40 Hz，该频率被下采样至 10 Hz，以及外部 Bosch BMI085 IMU，采样频率为 400 Hz。与此同时，机器人安装的序列配备了 Robosense BPearl5 激光雷达，采样频率为 10 Hz，八台全向相机，采样频率为 10 Hz，以及 Xsens MTi-670 IMU，采样频率为 200 Hz。在这两种情况下，都使用了面向前方的相机进行所有系统评估

(OS1: High-Res Mid-Range Lidar Sensor for Automation & Security | Ouster) (XT16/32/32M High-Precision 360° Mid-Range Lidar | Hesai) (https://www.robosense.ai/en/rslidar/RS-Bpearl)。在所有情况下，都提供了毫米级精度的真值，这些真值是通过运动捕捉系统（MoCap）或全站仪[^54^](https://ieeexplore.ieee.org/document/8509417)获得的。需要注意的是，Hilti 数据集的真值并未开源；因此，这些数据集上的算法结果是通过 Hilti 官方网站进行评估的。由于 Hilti'23 中的“Site 3”未提供深入分析图（例如，RMSE），我们排除了这四个序列，但这些序列的评分结果仍然可以在他们的官方网站(Hilti SLAM Challenge 2022 - Leaderboard) (https://hilti-challenge.com/leader-board-2023.html)上找到。NTU-VIRAL 和 Hilti 数据集共提供了 25 个序列(Design and Testbed Experiments of User Authentication and Key Establishment Mechanism for Smart Healthcare Cyber Physical Systems | IEEE Journals & Magazine | IEEE Xplore) (Tech Mining Analysis: Renewable Energy Forecasting Using Artificial Intelligence Technologies | IEEE Conference Publication | IEEE Xplore)

(https://ieeexplore.ieee.org/document/9803106)。

MARS-LVIG 数据集提供了高海拔、面向地面的测绘数据，涵盖了丛林、山脉和岛屿等多样化的无结构地形。该数据集是通过 DJI M300 RTK 四旋翼无人机收集的，该无人机配备了 Livox Avia

(https://www.livoxtech.com/avia)激光雷达（内置 BMI088 IMU）和高分辨率全局快门相机，两者触发频率均为 10 Hz。这与上述 NTU-VIRAL 和 Hilti 数据集明显不同，后者使用 752×480 灰度图像，而 MARS 数据集采用 2448×2048 RGB 图像，从而有助于生成清晰、密集的彩色点云。因此，我们利用这个公共数据集来验证我们在高海拔航空测绘应用中的能力

(https://arxiv.org/abs/2402.08239)。

B. FAST-LIVO2 私有数据集

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

平台：我们的数据收集平台如图9所示，配备了一台工业相机（MV-CA01321UC）、一台 Livox Avia 激光雷达和一台作为机载计算机的 DJI Manifold-2c（Intel i78550u CPU 和 8 GB RAM）。相机视场为 70.6°×68.5°，激光雷达视场为 70.4°×77.2°。所有传感器都通过 STM32 同步计时器生成的 10 Hz 触发信号进行硬件同步

(https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2%20supplementary.pdf)。
序列描述：如补充材料
(https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2%20supplementary.pdf)中的表 S1 所总结，FAST-LIVO2 私有数据集包含 20 个序列，涵盖了各种场景（例如，校园建筑、走廊、地下室、采矿隧道等），这些场景具有无结构、杂乱、昏暗、光照变化以及弱纹理环境的特征，总时长为 66.9 分钟。大多数序列都存在视觉和/或激光雷达退化的情况，例如，面对单一且/或无纹理的平面、穿越极其狭窄且/或黑暗的隧道以及从室内到室外的光照条件变化（见补充材料(https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2%20supplementary.pdf)中的图 S7）。为了保证在这些场景中增强同步数据收集，我们将相机配置为固定曝光时间但自动增益模式。对于其余采用自动曝光的序列，我们记录了它们的真实曝光时间。在所有序列中，平台都返回到起始点，从而可以评估漂移(https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2%20supplementary.pdf)。
IX. 实验结果

本节通过大量实验评估了我们提出的 FAST-LIVO2 系统。

A. 实现与系统配置

我们使用 C++ 和机器人操作系统（ROS）实现了 FAST-LIVO2 系统。在默认配置中，启用了曝光时间估计功能，而法向量细化功能则关闭。激光雷达扫描中的点按照 1:3 的时间比例进行下采样。体素地图的根体素尺寸设置为 0.5 米，内部八叉树的最大层级为 3。图像块的大小为 8×8（用于图像对齐）和 11×11（用于法向量细化）。在顺序 ESIKF 设置中，对于所有实验，相机光度噪声被设置为一个恒定值 100。激光雷达的深度误差和方位角误差根据不同的激光雷达型号进行了调整：Livox Avia 和 OS1-16 的深度误差为 0.02 米，方位角误差为 0.05°；PandarXT-32 的深度误差为 0.001 米，方位角误差为 0.001°；Robosense BPearl 的深度误差为 0.008 米，方位角误差为 0.01°。激光束发散角对于 Livox Avia 和 OS1-16 设置为 0.15°，对于 PandarXT-32 和 Robosense BPearl 设置为 0.001°。我们的系统在所有具有相同传感器配置的序列中使用相同的参数。所有实验的计算平台均为配备 Intel i7-10700K CPU 和 32 GB 内存的桌面计算机。此外，我们还在通常用于嵌入式系统的 ARM 处理器上测试了 FAST-LIVO2，该处理器功耗和成本较低。ARM 平台为 RB5，配备 Qualcomm Kryo585 CPU 和 8 GB 内存。我们将基于 ARM 平台的 FAST-LIVO2 实现称为“FAST-LIVO2 (ARM)”。

B. 基准实验

在本实验中，我们在 NTU-VIRAL、Hilti'22 和 Hilti'23 公共数据集的 25 个序列上进行了定量评估。我们的方法与几种最先进的开源里程计系统进行了基准比较，包括 R3LIVE [36]（一种密集直接激光雷达-惯性-视觉里程计系统）、FAST-LIO2 [13]（一种直接激光雷达-惯性里程计系统）、SDV-LOAM [40]（一种半直接激光雷达-视觉里程计系统）以及我们之前的工作 FAST-LIVO [8]。这些系统均从其各自的 GitHub 仓库下载。对于 FAST-LIO2、FAST-LIVO 和 LVI-SAM，我们使用了推荐的室内外场景设置，这些场景配备了多线激光雷达传感器。对于 R3LIVE，我们对其进行了调整，使其能够支持配备外部 IMU 的鱼眼相机模型和多线激光雷达（默认配置仅支持内部 IMU）。由于数据集中 IMU 激励不足，我们禁用了相机内参和外参 CTI 的实时优化，以避免出现不良优化。其他参数，包括光流跟踪的窗口大小、金字塔层级、当前扫描和全局地图的点云下采样分辨率等，均进行了微调以实现最佳性能。由于只有视觉模块的 SDV-LOAM 是开源的，我们按照原论文中描述的方法将其与 LeGO-LOAM [7] 进行了松耦合集成。这种增强系统继续优化视觉模块获得的位姿，我们也将这一实现公开在 GitHub 上(https://github.com/xuankuzcr/SDV-LOAM)。鉴于所有比较的系统均为无回环的里程计，除了 LVI-SAM 外，我们移除了 LVI-SAM 的回环模块，以确保公平比较。此外，我们还对曝光时间估计模块、法向量细化模块和参考块更新策略进行了消融研究。默认的 FAST-LIVO2 启用了实时曝光时间估计和参考块更新，但未启用法向量细化。

表 II 显示了所有方法的结果。可以看出，我们的方法在所有序列中的整体精度最高，平均均方根误差（RMSE）为 0.044 米，比排名第二的 FAST-LIVO（0.137 米）高出三倍。我们的系统在大多数序列中均取得了最佳结果，除了“Outside Building”和“Large Room (dark)”序列，在这两个序列中，我们的系统显示出略高（毫米级）的误差，与仅使用激光雷达-惯性里程计的 FAST-LIO2 相比。这种差异可以归因于这些序列具有丰富的结构特征，但光照条件较差，导致图像昏暗且模糊。因此，在融合这些低质量图像时，并未提高里程计的精度。除了这两个序列外，我们的系统利用紧密耦合的激光雷达、惯性和视觉信息，在其他序列中显著优于 FAST-LIO2（我们的 LIO 子系统）和仅使用激光雷达-视觉的 SDV-LOAM。值得注意的是，SDV-LOAM 在 Hilti 数据集上的表现特别差，这是因为它缺乏与 IMU 测量的紧密集成，导致 LO 子系统出现漂移。此外，视觉和激光雷达观测之间的松耦合，以及视觉里程计的初始值较差，常常导致局部最优甚至负优化。我们的 LIO 子系统通常优于 FAST-LIO2，这归功于我们对每个激光雷达点的噪声进行了更准确的建模。在少数 FAST-LIO2 略微优于我们的序列中，差异仅为毫米级，可以忽略不计。此外，我们的系统在所有序列中的精度显著优于其他紧密耦合的激光雷达-惯性-视觉系统。其中，LVI-SAM 在九个序列中失败，主要原因是其基于特征的 LIO 和 VIO 子系统未能充分利用原始测量数据，从而降低了其在具有细微几何结构或纹理特征的环境中的鲁棒性。R3LIVE 总体表现良好，但在“Construction Stairs”、“Cupola”和“Attic to Upper Gallery”序列中表现不佳，其性能甚至比 FAST-LIO2 更差。这是因为在结构缺失的楼梯处的剧烈旋转导致姿态先验不足，从而在将彩色地图点与当前帧对齐时出现局部最优，最终导致负优化。FAST-LIVO 和 FAST-LIVO2 通过基于块的图像对齐克服了这些序列中的挑战。此外，在这些序列中传感器靠近墙壁的情况突显了 FAST-LIVO2 中射线投射的有效性，这些大规模场景的建图结果如补充材料(https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2%20supplementary.pdf)中的图 S8 所示。另一方面，FAST-LIVO 在 NTU-VIRAL 数据集上的表现不如 R3LIVE 和 FAST-LIVO2，尤其是在结构缺失的“nya”序列中，这是因为在这些序列中，基于恒定深度假设的仿射变换效果不准确。相比之下，R3LIVE 的像素级对齐和 FAST-LIVO2 的平面先验（或细化）并未遇到此类问题。与 FAST-LIVO 相比，FAST-LIVO2 克服了这些挑战，通过基于块的图像对齐显著提高了这些序列的性能。此外，与 R3LIVE 相比，FAST-LIVO2 的平均精度提高了 1 毫米，而精度提升并不一致。有限的改进主要是因为在简单结构场景中，具有良好的图像观测时，法向量细化才会有正优化。在 NTU-VIRAL 数据集中，“eee”和“nya”序列的图像极为昏暗且模糊，负优化尤为严重。为了进一步研究不同模块（包括曝光时间估计、仿射变换、参考块更新、法向量收敛、按需射线投射和 ESIKF 顺序更新）的有效性，我们在我们的私有数据集和 MARS-LVIG 数据集上进行了全面研究。由于篇幅限制，结果在补充材料(https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2%20supplementary.pdf)的 I（系统模块验证）部分进行了展示。正如结果所证实的那样，我们的系统能够在结构化和非结构化环境中实现鲁棒且准确的姿态估计，在严重光照变化下、在大规模场景中进行长期高速数据采集，甚至在极其狭窄的空间中，激光雷达测量极少的情况下，都能表现出色。

C. 激光雷达退化和视觉挑战环境

在本实验中，我们在经历激光雷达退化和/或视觉挑战的环境中评估了系统的鲁棒性，并与 FAST-LIVO 和 R3LIVE 的定性建图结果进行了比较，如图 10 和 11 所示。图 10 展示了激光雷达退化序列，激光雷达在沿着墙壁从一侧移动到另一侧时始终面对一面大墙。由于缺乏几何约束（因为激光雷达只观察到一个平面），LIO 方法会失败。值得一提的是，“HIT Graffiti Wall”序列的跨度接近 800 米，激光雷达在整个过程中始终面对墙壁，导致严重的退化。在所有序列中，FAST-LIVO2 显著展示了其在长期退化下的鲁棒性，并能够提供高精度的彩色点云地图。相比之下，FAST-LIVO 虽然能够获得几何结构，但纹理完全模糊。R3LIVE 在几何结构和纹理清晰度方面都存在困难。图 11 展示了更复杂的场景测试，其中激光雷达和/或相机偶尔会退化。退化方向由相应的箭头指示。“HKU Cultural Center”（图 11（a））展示了 FAST-LIVO2、R3LIVE 和 FAST-LIVO 的建图结果。如图所示，R3LIVE 和 FAST-LIVO 的点云地图出现扭曲，纹理模糊，漂移超过 1 米。相比之下，FAST-LIVO2 成功返回到起始点，实现了令人印象深刻的端到端误差小于 0.01 米，同时保持了一致的点云地图和清晰的纹理。“CBD Building 03”（图 11（b））和“Mining Tunnel”（图 11（c））仅展示了 FAST-LIVO2 的结果，因为 R3LIVE 和 FAST-LIVO 均已失败。在图 11（b）中，蓝色箭头表示朝向纯黑屏幕的移动方向，表明激光雷达和相机同时退化。在图 11（c1）和（c2）中，红色点表示该位置的激光雷达扫描，说明了由于观察到单一平面而导致的激光雷达退化区域。此外，“Mining Tunnel”在整个序列中都存在非常昏暗的光照，同时激光雷达和相机频繁退化。尽管面临这些挑战，FAST-LIVO2 仍然能够返回到起始点，端到端误差小于 0.01 米。

D. 高精度建图

在本实验中，我们验证了系统的高精度建图能力。为了在不同算法之间公平地探索建图精度，我们比较了我们的系统与 FAST-LIO2、R3LIVE 和 FAST-LIVO 在具有丰富纹理和结构化环境的场景中的表现。我们以“SYSU 01”、“HKU Landmark”和“CBD Building 01”为例。补充材料(https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2%20supplementary.pdf)中的图 S9 展示了这些序列的实时建图结果。我们可以清楚地看到，FAST-LIVO2 生成的点云地图在所有系统中保留了最精细的细节，放大后的彩色点云地图与实际 RGB 图像相似。在“SYSU 01”序列中，我们的算法在标志牌上产生的白噪声点较少，因为我们通过使用恢复的曝光时间对图像颜色进行归一化，从而在着色前避免了过曝的彩色点云地图。在“CBD Building 01”中重建的人和摩托车也展示了我们重建无结构物体细节的能力。在所有序列中，最终估计的位置都能返回到起始点，端到端误差小于 0.01 米。我们还在私有数据集的其余序列上测试了 FAST-LIVO2，建图结果如补充材料(https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2%20supplementary.pdf)中的图 S10-S13 所示。

E. 运行时间分析

在本节中，我们在配备 Intel i7-10700K CPU 和 32 GB 内存的桌面计算机上评估了我们提出的系统在每个激光雷达扫描和图像帧上的平均计算时间。我们的评估涵盖了公共数据集，包括 Hilti'22、Hilti'23 和 NTU-VIRAL，以及我们的私有数据集。如表 III 所示，我们的系统在所有序列中的处理时间均为最低。在 Intel i7 处理器上，平均处理时间仅为 30.03 毫秒（每个激光雷达扫描 17.13 毫秒，每个图像帧 12.90 毫秒），能够以 10 Hz 的频率实现实时运行。此外，我们的系统甚至能够在 ARM 处理器上实现实时运行，每个帧的平均处理时间为 78.44 毫秒。LVI-SAM 的激光雷达和视觉特征提取模块在 LIO 和 VIO 中耗时较长。除了 LIO 和 VIO 所消耗的时间外，LVI-SAM 还在因子图中整合了 IMU 预积分约束、视觉里程计约束和激光雷达里程计约束，进一步增加了整体处理时间。对于 R3LIVE，尽管也采用了直接方法，但其像素级图像对齐需要使用大量的视觉地图点。相比之下，我们的方法使用稀疏点和参考块，能够高效对齐。此外，R3LIVE 维护了一个彩色地图，该地图会进行贝叶斯更新，随着地图分辨率的提高，计算负担显著增加。对于 FAST-LIO2，表 S3（由于篇幅限制，补充材料(https://github.com/hku-mars/FAST-LIVO2/blob/main/Supplementary/LIVO2%20supplementary.pdf)中未显示）中的平均每个帧的处理时间约为 10.35 毫秒，比 FAST-LIVO2 少，因为没有处理额外的图像测量。FAST-LIVO2 也比前身 FAST-LIVO 有显著改进。主要改进来自于我们在稀疏图像对齐中应用了逆组合公式。基于激光雷达点的平面先验的仿射变换进一步提高了我们方法的收敛效率。因此，FAST-LIVO2 将每个金字塔层级的迭代次数从 10 次减少到 3 次，同时仍然实现了更高的精度。

X. 应用

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

A. 完全机载自主无人机导航

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

系统配置：硬件和软件设置如图 12 所示。在硬件方面，我们使用配备 Intel i7-1360P CPU 和 32 GB 内存的 NUC 作为机载计算机。在软件方面，定位组件由 FAST-LIVO2 提供，以 10Hz 的频率提供位置反馈。定位结果被送入飞行控制器，以实现 200Hz 的位置、速度和姿态反馈。 FAST-LIVO2 轨迹规划器 MPC 控制器角速度控制器扩展卡尔曼滤波器混合器密集点云位置估计轨迹指令期望角速度期望推力点云和 IMU （10 Hz 和 200 Hz，激光雷达）图像（10 Hz）电机 PWM 位置、速度、姿态反馈（200 Hz） IMU（200 Hz，飞行控制）期望力矩角速度反馈飞行控制器机载计算机位置反馈（10 Hz）电池 STM32 内置 IMU 激光雷达相机机载 PC 图 12：完全机载无人机导航算法流程图。除了定位，FAST-LIVO2 还为规划模块提供了一个密集的注册点云，该模块是一个气泡规划器 [55]，它规划一条平滑的轨迹，然后由基于流形的模型预测控制器（MPC）[56] 追踪。MPC 计算期望的角速率和推力，这些推力由飞行控制器上的低级角速率控制器追踪。重要的是，MPC、规划器和 FAST-LIVO2 都在机载计算机上实时运行。
无人机自主导航：我们进行了 4 次完全机载自主无人机导航实验，分别是“地下室”、“树林”、“狭窄通道”和“SYSU 校园”（见补充材料 [53] 中的表 S2）。其中，“地下室”和“树林”展示了无人机的成功自主导航和避障能力。在“狭窄通道”中，无人机被命令靠近墙壁飞行，导致激光雷达点测量极少。然而，射线投射模块提供了足够的视觉约束，以缓解退化，从而实现稳定的定位。此外，“地下室”和“狭窄通道”经历了激光雷达退化，只观察到一面墙（见图 1（e1）和（e4）、图 13（b1-b4）），以及显著的曝光变化（见图 1（e5-e6））。尽管面临这些挑战，我们的无人机系统表现依然出色。“树林”中，无人机以高达 3 米/秒的速度移动，要求整个无人机系统能够快速响应（见图 13（a1-a4））。“SYSU 校园”是一个非退化场景，主要展示了机载高精度建图能力（见补充材料 [53] 中的图 S14）。最后，值得注意的是，在这四次无人机飞行中，都发生了严重的光照变化。FAST-LIVO2 能够估计曝光时间，与真实值非常接近（见补充材料 [53] 中的图 S15）。关于机载计算时间，由于需要在机载计算机上同时运行 MPC（100 Hz）和规划（10 Hz），这消耗了计算资源和内存，限制了可供 FAST-LIVO2 使用的计算资源。尽管如此，如图 14 所示，FAST-LIVO2 的平均机载处理时间约为 53.47 毫秒，远低于 100 毫秒的帧周期。MPC 和规划的平均处理时间分别为 18.5 毫秒和 8.43 毫秒。总平均处理时间为 80.4 毫秒，完全满足机载操作的实时要求。 FAST-LIVO2 规划 MPC（10 次）总时间 0 20 40 60 80 时间（毫秒）机载无人机实验时间成本图 14：在“地下室”、“树林”、“狭窄通道”和“SYSU 校园”实验中，每个模块以及总体的处理时间。MPC 以 100 Hz 的频率运行，而规划和 FAST-LIVO2 以 10 Hz 的频率运行，因此其计算时间被计算了 10 次。

B. 航空测绘

航空测绘是测绘和制图应用中的一个重要任务。为了评估 FAST-LIVO2 在这一应用中的适用性，我们使用公共数据集 MARS-LVIG [52] 进行了一次航空测绘实验，其硬件配置在第八节 A 部分中进行了详细说明。我们在“HKairport01”和“HKisland01”两个序列上评估了实时建图结果，如图 1（a-c）所示，其中（a）和（c）对应于“HKisland01”，（b）对应于“HKairport01”。结果表明，FAST-LIVO2 在森林和岛屿等无结构环境中表现出色。系统成功捕捉到了许多精细结构和鲜明的着色效果，包括建筑物、道路上的车道标记、道路边缘、树冠和岩石，这些细节都清晰可见。这些序列的 APE（RMSE）分别为 FAST-LIVO2 的 0.64 米和 0.27 米，而 R3LIVE 分别为 2.76 米和 0.52 米。在桌面计算机（第九节 A 部分）上的平均处理时间分别约为 25.2 毫秒和 21.8 毫秒，而 R3LIVE 分别为 110.5 毫秒和 100.2 毫秒。

C. 支持 3D 场景应用：网格生成、纹理和高斯点云

利用 FAST-LIVO2 提供的高精度传感器定位和密集的彩色点云地图，我们开发了渲染流程中的软件应用，包括网格化和纹理映射，以及新兴的基于神经辐射场（NeRF）的渲染流程，例如 3D 高斯点云（3DGS）。对于网格化，我们基于截断符号距离函数（TSDF）使用 VDBFusion [57] 在“CBD Building 01”中进行操作。柱子的锐边和屋顶的独特结构清晰可见，这证明了网格的高质量。这种细节水平的实现归功于 FAST-LIVO2 的点云密度高和结构重建精度高。在网格构建完成后，我们使用 OpenMVS [58] 在“CBD Building 01”和“Retail Street”中进行纹理映射，使用估计的相机位姿。在图 15（c1-c2）中，应用于三角面片的纹理图像无缝且精确对齐，从而实现了高度清晰和精确的纹理映射。这归功于 FAST-LIVO2 实现的像素级图像对齐。FAST-LIVO2 的密集彩色点云也可以直接作为 3DGS 的输入。我们在“CBD Building 01”序列上进行了测试，使用了总共 1180 张图像中的 300 帧。结果如图 16 所示。与 COLMAP [59] 相比，我们的方法显著减少了从 9 小时缩短到 21 秒的时间，用于从头开始获取密集点云和位姿。然而，训练时间从 10 分钟 59 秒增加到了 15 分钟 30 秒。这种增加归因于更密集的点云（下采样到 5 厘米），这引入了更多的参数进行优化。尽管如此，我们更密集和精确的点云导致比从 COLMAP 输入获得的峰值信噪比（PSNR）略高。