# FAST-LIVO2：快速、直接的激光雷达-惯性-视觉里程计
郑春然 $^{ID}$，IEEE学生会员；徐伟 $^{ID}$；邹祖浩 $^{ID}$；华桐 $^{ID}$；袁崇健 $^{ID}$；何东娇 $^{ID}$，IEEE会员；周炳阳，IEEE会员；刘峥 $^{ID}$，IEEE会员；林家荣 $^{ID}$，IEEE会员；朱方成 $^{ID}$，IEEE学生会员；任云帆 $^{ID}$，IEEE学生会员；王蓉、孟凡乐、张福 $^{ID}$，IEEE会员。
摘要—本文提出FAST-LIVO2，这是一种快速且直接的激光雷达-惯性-视觉里程计框架，旨在为SLAM任务提供精确、鲁棒的状态估计，从而支持实时机器人应用。FAST-LIVO2通过高效的误差状态迭代卡尔曼滤波器（ESIKF）融合惯性测量单元（IMU）、激光雷达和图像数据。为解决激光雷达与图像测量之间的维度不匹配问题，我们采用了顺序更新策略。通过直接法处理激光雷达和视觉数据融合进一步提升了效率：激光雷达模块无需提取特征即可直接配准原始点云，而视觉模块无需依赖特征提取即可最小化光度误差。激光雷达与视觉测量结果会被融合到统一的体素地图中：激光雷达模块负责构建几何结构，视觉模块则将图像块与激光雷达点关联起来，从而实现精确的图像对齐。来自激光雷达点的平面先验可提升对齐精度，且在过程中会动态优化该先验。此外，按需光线投射操作和实时图像曝光估计进一步增强了系统的鲁棒性。在基准数据集和自定义数据集上的大量实验表明，FAST-LIVO2在精度、鲁棒性和效率方面均优于现有最先进的系统。我们对关键模块进行了验证，并展示了三项应用场景：凸显实时能力的无人机导航、体现高精度的航空测绘，以及基于网格和NeRF的3D模型渲染，证明了其在密集建图领域的适用性。相关代码和数据集已在GitHub上开源，以供机器人领域研究者使用。
索引词—三维重建、空中导航、传感器融合、同时定位与建图（SLAM）。
## 一、引言
近年来，同时定位与建图（SLAM）技术 $^{1}$ 取得了显著进展，尤其是在实时三维重建方面。视觉SLAM利用成本较低的CMOS传感器和镜头，能够建立准确的数据关联，进而实现一定水平的定位精度；丰富的色彩信息也进一步丰富了语义感知能力。借助这种增强的场景理解能力，深度学习方法可被用于鲁棒的特征提取和动态物体过滤。然而，视觉SLAM缺乏直接的深度测量能力，需要通过三角测量或深度滤波等操作同步优化地图点，这会带来巨大的计算开销，往往限制了地图的精度和密度。视觉SLAM还存在诸多其他局限，比如不同尺度下的测量噪声存在差异、对光照变化敏感，以及无纹理环境会影响数据关联等。
目前，已有多个SLAM框架借助单传感器成功实现，这些传感器主要是摄像头 [^1]、[^2]、[^3]、[^4]，或是激光雷达 [^5]、[^6]、[^7]。尽管视觉SLAM和激光雷达SLAM各自在其适用领域展现出潜力，但二者都存在固有局限性，限制了它们在不同场景下的性能表现。
未知环境。由于SLAM能够实时估计位姿并重建地图，它已成为各类机器人导航任务中不可或缺的技术。定位过程为机器人的机载控制器提供关键的状态反馈，而稠密的三维地图则提供了诸如空闲空间和障碍物等关键环境信息，这些信息对于有效的轨迹规划至关重要。彩色地图还承载着丰富的语义信息，能够生动呈现现实世界，从而衍生出广阔的应用前景，比如虚拟现实与增强现实、三维建模以及人机交互等。
激光雷达SLAM利用激光雷达传感器直接获取精确的深度测量数据，在定位和绘图任务中的精度与效率均优于视觉SLAM。尽管有这些优势，激光雷达SLAM仍存在若干显著缺陷。一方面，它所重建的点云地图虽然细节丰富，却缺乏颜色信息，因此信息量有限；另一方面，在几何约束不足的环境中，比如狭窄隧道或单一且延长的墙面处，激光雷达SLAM的性能往往会下降。
随着智能机器人在现实世界中运行的需求日益增长，尤其是在那些缺乏结构或纹理的环境中，人们逐渐意识到：依赖单一传感器的现有系统无法提供所需的精确且稳健的位姿估计。为解决这一问题，多传感器融合技术正受到越来越多的关注。
---
常用的传感器，如激光雷达、摄像头和惯性测量单元（IMU），其融合应用正日益受到重视。这种策略不仅能结合这些传感器的优势以提升位姿估计精度，还能即使在单个传感器性能受限的环境中，也能构建出精确、稠密且带有颜色的点云地图。
实现高效且精确的激光雷达-惯性-视觉里程计及建图仍是极具挑战性的问题。
1) 整个LIVO系统需要处理每秒数百到数千个点的激光雷达测量数据，以及高帧率、高分辨率的图像数据。要在有限的机载资源下充分利用如此庞大的数据量，对计算效率提出了极高的要求。
2) 许多现有系统通常包含激光雷达-惯性里程计（LIO）子系统和视觉-惯性里程计（VIO）子系统，每个子系统都需要分别从视觉数据和激光雷达数据中提取特征以减轻计算负担。在缺乏结构或纹理的环境中，这一提取过程往往只能得到有限的特征点。此外，为了优化特征提取效果，还需进行大量的工程调整，以适应激光雷达扫描模式和点密度的差异性。
3) 为了降低计算需求并实现摄像头与激光雷达测量数据之间的更紧密集成，必须建立统一的地图来同时管理稀疏的点云数据和观测到的高分辨率图像数据。然而，考虑到激光雷达和摄像头数据的异质性，设计和维护此类地图尤为困难。
4) 为确保重建的彩色点云具有高精度，姿态估计必须达到像素级精度。实现这一标准面临巨大挑战：需要精确的硬件同步、激光雷达与摄像头之间外部参数的严格预校准、曝光时间的精准恢复，以及能够实时达到像素级精度的融合策略。
基于上述问题，我们提出了FAST-LIVO2——一种高效的LIVO系统。该系统通过顺序更新的误差状态迭代卡尔曼滤波器（ESIKF）将激光雷达、图像和IMU测量数据紧密集成。借助IMU传播的先验信息，系统状态按顺序更新：先利用激光雷达测量数据，再使用图像测量数据，两者均基于统一的体素图采用直接法进行处理。具体而言，在激光雷达更新阶段，系统将原始点云注册到地图中，以构建并更新其几何结构；在视觉更新阶段，系统直接将激光雷达地图中的点作为视觉地图点，无需从图像中提取、三角测量或优化任何视觉特征。地图中选定的视觉地图点会附带之前观测到的参考图像块，随后将其投影到当前图像上，通过最小化直接光度误差来实现姿态对齐（即稀疏图像对齐）。为提高图像对齐的精度，FAST-LIVO2会动态更新参考图像块，并利用从激光雷达点获取的平面先验信息。为提升计算效率，FAST-LIVO2会利用激光雷达点来识别当前图像中可见的视觉地图点；若没有激光雷达点，则按需进行体素光线投射。此外，FAST-LIVO2还能实时估算曝光时间，以应对光照变化。
FAST-LIVO2是基于我们此前工作中提出的FAST-LIVO开发的[^8]。与FAST-LIVO相比，它的新贡献如下。
1) 我们提出了一种采用顺序更新的高效ESIKF框架，以解决激光雷达与视觉测量之间的维度不匹配问题，从而提升了采用异步更新的FAST-LIVO的鲁棒性。
2) 我们利用（甚至优化了）来自激光雷达点的平面先验信息以提高精度。相比之下，FAST-LIVO假设图像块中的所有像素具有相同的深度，这一过于激进的假设会大幅降低图像对齐中仿射变换的精度。
3) 我们提出了一种参考图像块更新策略以提高图像对齐的精度，该策略会选择视差大且纹理细节丰富的优质内点参考图像块。FAST-LIVO则是根据与当前视图的接近程度来选择参考图像块，往往会导致参考图像块质量较低，进而影响对齐精度。
4) 我们进行了在线曝光时间估算，以应对环境光照变化。FAST-LIVO并未解决这一问题，因此在光照变化显著时图像对齐的收敛性较差。
5) 我们提出了按需体素光线投射技术，以增强系统在因激光雷达近距离盲区导致缺乏激光雷达点测量数据时的鲁棒性，这一问题在FAST-LIVO中并未被考虑。
上述每一项贡献都通过全面的消融实验进行了评估，以验证其有效性。我们将所提出的系统开发为实用的开源软件，针对Intel和ARM处理器进行了精细优化以实现实时运行。该系统功能多样，支持多线旋转式激光雷达、具有非传统扫描模式的新兴固态激光雷达，以及针孔相机和各种鱼眼相机。
此外，我们在25个公开数据集序列（即Hilti和NTU-VIRAL数据集）以及各种具有代表性的私有数据集上开展了大量实验，从而能够与其他最先进的SLAM系统（如R3LIVE、LVI-SAM、FAST-LIO2等）进行比较。定性和定量结果均表明：所提出的系统在精度与鲁棒性方面显著优于其他同类系统，同时计算成本更低。
为了进一步凸显我们系统的实际应用价值与多功能性，我们部署了三种不同的应用场景。首先，全机载自主无人机导航演示了该系统的实时处理能力，这堪称利用激光雷达-惯性-视觉系统实现真实世界无人机飞行的开创性实例。其次，航空测绘展示了该系统在实际无结构环境下的像素级精度。最后，高质量网格、纹理及NeRF模型的生成则体现了该系统在渲染任务中的适用性。我们的代码和数据集已在GitHub上开源。
### 二、相关工作
### A. 直接法
直接法作为一种突出的方法，在视觉和LiDAR SLAM中用于快速位姿估计。与基于特征的方法 [^5], [^6], [^9], [^10] 不同，后者需要
---
提取显著特征点（例如图像中的角点和边缘像素；LiDAR扫描中的平面点和边缘点）并生成用于匹配的鲁棒描述子，直接法直接利用原始测量来优化传感器位姿 [^11]，通过最小化基于光度误差或点到平面残差的误差函数，例如 [^3], [^12], [^13], [^14]。通过消除耗时的特征提取与匹配过程，直接法可实现快速位姿估计。然而，缺乏特征匹配要求具有相当准确的状态先验估计以避免陷入局部极小值。
视觉SLAM中的直接法可大致分为稠密直接法、半稠密直接法和稀疏直接法。稠密直接法主要应用于具有完整深度测量的RGB-D相机，如 [^15], [^16] 和 [^17] 中作者所述，采用图像到模型对齐进行位姿估计。相比之下，半稠密直接法 [^3], [^18] 利用具有显著灰度梯度的像素实现直接图像对齐以进行估计。稀疏直接法 [^2], [^12] 专注于通过仅使用少量精心选取的原始图像块来提供准确的状态估计，从而相较于稠密和半稠密直接法进一步降低了计算负担。
与直接视觉SLAM方法不同，直接激光雷达SLAM系统[^13]、[^14]、[^19]、[^20]并不区分密集和稀疏的处理方式，通常会利用每次扫描中经过空间下采样或时间下采样的原始点来构建用于位姿优化的约束条件。
在我们的工作中，我们对激光雷达和视觉模块均采用了直接法的原理。系统的激光雷达模块源自VoxelMap [^14]，而视觉模型则基于稀疏直接法的一种变体 [^12]。虽然我们的视觉模块从[^12]中的稀疏直接图像对齐技术中汲取灵感，但它有所不同：它重新利用激光雷达点作为视觉地图点，从而减轻了后端计算的负担（即特征对齐、滑动窗口优化和/或深度滤波）。
### B. 激光雷达-视觉（-惯性）SLAM
在激光雷达-视觉-惯性SLAM中引入多种传感器，使系统能够应对各种复杂环境，尤其是当某一传感器出现故障或部分退化时。受此启发，研究界涌现出了多种激光雷达-视觉-惯性SLAM系统。现有方法大致可分为两类：松耦合和紧耦合。这种分类可从两个角度来界定：状态估计层面和原始测量层面。在状态估计层面，关键在于一种传感器的估计值是否作为另一种传感器模型中的优化目标；在原始测量层面，则涉及是否将不同传感器的原始数据加以融合。
Zhang等人 [^21] 提出了一种在状态估计层面属于松耦合的激光雷达-视觉-惯性SLAM系统。在该系统中，VIO子系统仅为LIO子系统中的扫描配准提供初始位姿，而非与扫描配准过程进行联合优化。VIL-SLAM [^22] 采用了类似的松耦合方法，并未对激光雷达、相机和IMU的测量数据进行联合优化。
一些系统（如DEMO [^23]、LIMO [^24]、CamVox [^25]、[^26]）利用三维激光雷达点为视觉模块提供深度测量值 [^1]、[^4]、[^27]。虽然这些系统在测量层面实现了紧耦合，但在状态估计层面仍属松耦合，主要是因为缺乏直接在状态估计阶段源于激光雷达测量的约束。另一个问题是，由于分辨率不匹配，三维激光雷达点与二维图像特征点和/或线条之间不存在一一对应关系。这种不匹配需要在深度关联时进行插值处理，从而可能引入误差。为解决这一问题，DVL-SLAM [^28] 采用了直接法进行视觉跟踪，即将激光雷达点直接投影到图像中以确定对应像素位置的深度。
此前提及的相关研究并未在状态估计层面实现紧密耦合。为了追求更高的精度和鲁棒性，近期涌现了许多以紧密耦合方式联合优化传感器数据的研究。例如，LIC-Fusion [^29] 基于MSCKF [^30] 框架，将IMU测量数据、稀疏视觉特征以及激光雷达的平面和边缘特征进行紧密融合。后续的LIC-Fusion2.0 [^31] 通过在滑动窗口内实施平面特征跟踪，进一步提升了激光雷达的位姿估计精度。VILENS [^32] 则依托固定滞后平滑技术，通过统一的因子图对视觉、激光雷达及惯性数据进行联合优化。R2LIVE [^33] 利用流形上的迭代卡尔曼滤波器 [^34] 将激光雷达、相机和IMU测量数据紧密融合。对于R2LIVE中的VIO子系统而言，它采用滑动窗口优化方法来确定地图中视觉特征的位置。
有几套系统同时在测量和状态估计层面实现了完全的紧密耦合。LVI-SAM [^35] 在基于因子图构建的紧密耦合平滑与建图框架中，融合了激光雷达、视觉及惯性传感器数据。其VIO子系统负责视觉特征跟踪，并利用激光雷达扫描结果提取特征深度。R3LIVE [^36] 通过LIO构建全局地图的几何结构，同时借助VIO渲染地图纹理。这两个子系统通过将各自的激光雷达或视觉数据与IMU信息相融合，共同估计系统状态。其升级版本R3LIVE++ [^37] 能够实时估算曝光时间并提前进行光度校准 [^38]，从而使系统能够恢复地图点的辐射度。与大多数此前提及的激光雷达-惯性-视觉系统不同，这些系统对LIO和VIO子系统均采用基于特征的方法；而R3LIVE系列 [^36]、[^37] 则对两者均采用直接法，无需进行特征提取，因此即便在缺乏纹理或结构的场景中也能捕捉到细微的环境特征。
我们的系统同样利用激光雷达、图像及IMU数据来联合估计状态，并在测量层面维护一个紧密耦合的体素地图。此外，我们的系统采用直接法，利用原始激光雷达点进行激光雷达扫描配准，并使用原始图像块进行视觉跟踪。我们的系统与R3LIVE（或R3LIVE++）的关键区别在于：R3LIVE（及R3LIVE++）在VIO中是在单个像素级别进行处理，而我们的系统则在图像块级别进行操作。这一差异赋予了我们的系统显著优势。首先，在鲁棒性方面，我们的方法采用简化的单步帧到地图稀疏图像对齐来进行位姿估计，从而减轻了对精确初始状态的依赖——这种精确初始状态在R3LIVE中需通过帧间光流来获取。因此，我们的系统简化并优化了R3LIVE中的两步帧间及帧到地图操作。其次，从计算角度来看，R3LIVE中的VIO主要采用密集直接法，
---
Fig. 1. FAST-LIVO2 mapping results generated in real time. (a)–(c) showcase airborne mapping, (d) represents a retail street collected with a handheld device, and (e) demonstrates an experiment where a UAV carrying a LiDAR, camera, and inertial sensor perform real-time state estimation (i.e., FAST-LIVO2), trajectory planning, and tracking control all on its onboard computer. In (d)–(e), blue lines represent the computed trajectory. In (e)–(f), white points indicate the LiDAR scan at that moment, and colored lines depict the planned trajectory. (e1) and (e4) mark areas of LiDAR degeneration. (e2) and (e3) show obstacle avoidance. (e5) and (e6) depict the camera first-person view from indoor to outdoor, highlighting large illumination variation from sudden overexposure to normal (see our accompanying video on YouTube). $^{2}$
这种方法计算开销较大，需要大量的点来构建残差并进行渲染。相比之下，我们的稀疏直接法具有更高的计算效率。最后，我们的系统利用了原始图像块分辨率层面的信息，而R3LIVE的信息分辨率则受限于其点云地图。
我们系统的视觉模块与DV-LOAM [^39]、SDV-LOAM [^40]以及LVIO-Fusion [^41]最为相似，它们都是将带有图像块的激光雷达点投影到新图像中，并通过最小化直接光度误差来跟踪该图像。不过，它们之间存在几个关键差异：视觉模块和激光雷达分别使用独立的地图；在图像块扭曲过程中依赖深度恒定的假设；在状态估计层面耦合较为松散；图像对齐分为帧间对齐和帧到关键帧对齐两个阶段。相比之下，我们的系统在迭代卡尔曼滤波器中紧密集成了帧到地图的图像对齐、激光雷达扫描配准以及惯性测量单元数据。此外，由于激光雷达和视觉模块共用同一张统一地图，我们的系统能够直接利用激光雷达点提供的平面先验来加速图像对齐。
### 三、系统概述
图2展示了我们系统的概览，该系统包含四个部分：ESIKF（见第四节）、局部建图（见第五节）、激光雷达测量模型（见第六节）以及视觉测量模型（见第七节）。
异步采样的激光雷达点首先通过扫描重组，在相机采样时刻重新组合成扫描数据。随后，我们通过具有顺序状态更新的ESIKF将激光雷达、图像和惯性测量数据紧密耦合；系统状态按顺序更新，先利用激光雷达测量数据，再使用图像测量数据，两者都基于同一张统一体素地图采用直接法（详见第四节）。为了在ESIKF更新中构建激光雷达测量模型（见第六节），我们计算帧到地图的点到平面残差。为建立视觉测量模型（见第七节），我们从地图中提取当前视场内的视觉地图点，这一过程借助可见体素查询和按需光线投射技术；提取完成后，我们会识别并丢弃异常的视觉地图点（例如被遮挡或存在深度不连续性的点）；之后计算帧到地图的图像光度误差以进行视觉更新。
用于视觉和激光雷达更新的局部地图是体素地图结构（见第五节）：激光雷达点负责构建并
---
Fig. 2. System overview of FAST-LIVO2.
TABLE I SOME IMPORTANT NOTATIONS
| Notations | Explanation |
| $\boxplus$ / $\boxminus$ | The encapsulated “boxplus” and “boxminus” operations on the state manifold |
| $^{G}(\cdot)$ | A vector ( $\cdot$) in global world frame |
| $^{C}(\cdot)$ | A vector ( $\cdot$) in camera frame |
| $^{I}T_{L}$ | The extrinsic of LiDAR frame w.r.t. IMU frame |
| $^{C}T_{I}$ | The extrinsic of IMU frame w.r.t. camera frame |
| $^{G}T_{I}$ | The pose of IMU frame at time k w.r.t. the global frame |
| $\mathbf{x}, \widehat{\mathbf{x}}, \bar{\mathbf{x}}$ | The ground-truth, predicted and updated estimation of $\mathbf{x}$ |
| $\bar{\mathbf{x}}^{k}$ | The $\kappa$-th update of $\mathbf{x}$ |
| $\delta\mathbf{x}$ | The error state between ground-truth $\mathbf{x}$ and its estimation |
更新地图的几何结构，而视觉图像则将图像块添加到选定的地图点上（即视觉地图点），并动态更新参考图像块。更新后的参考图像块的法向量会在独立线程中进一步细化。
### 四、具有顺序状态更新的ESIKF
本节将介绍基于顺序更新ESIKF框架的系统架构。
### A. 符号说明与状态转移模型
在我们的系统中，假设三种传感器（激光雷达、惯性测量单元和相机）之间的时间偏移已知，这些偏移可以提前校准或同步。我们以惯性测量单元坐标系（记为I）作为机体坐标系，第一个机体坐标系作为全局坐标系（记为G）。此外，我们假设这三种传感器刚性连接，表I中定义的外部参数已预先校准。那么，在第i次惯性测量时的离散状态转移模型为
$$ \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) $$
其中$\Delta t$表示惯性测量单元采样周期，状态x、输入u、过程噪声w以及函数f的定义如下：
$$ \mathcal{M}\triangleq S O(3)\times\mathbb{R}^{16},\operatorname{d i m}(\mathcal{M})=19 $$
$$ \mathbf{x}\triangleq\left[\begin{matrix}{^{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{matrix}\right]^{T}\in\mathcal{M} $$
$$ \mathbf{u}\triangleq\left[\begin{matrix}{\mathbf{\omega}_{m}^{T}}&{\mathbf{a}_{m}^{T}}\\ \end{matrix}\right]^{T},\mathbf{w}\triangleq\left[\begin{matrix}{\mathbf{n}_{g}^{T}}&{\mathbf{n}_{a}^{T}}&{\mathbf{n}_{b_{g}}^{T}}&{\mathbf{n}_{b_{a}}^{T}}&{n_{\tau}}\\ \end{matrix}\right]^{T} $$
Fig. 3. Illustration of scan recombination, forward propagation and backward propagation applied to input data.
$$ \mathbf{f}(\mathbf{x},\mathbf{u},\mathbf{w})=\left[\begin{matrix}{\mathbf{\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\times1}}\\ {n_{\tau}}\\ \end{matrix}\right] $$
其中，$^G\mathbf{R}_I$、$^G\mathbf{p}_I$ 和 $^G\mathbf{v}_I$ 分别表示全局坐标系下的IMU姿态、位置和速度，$^G\mathbf{g}$ 是全局坐标系下的重力矢量，$\tau$ 是相对于第一帧的相机曝光时间倒数，$n_\tau$ 是用来对 $\tau$ 进行随机游走建模的高斯噪声，$\omega_m$ 和 $\mathbf{a}_m$ 是原始的IMU测量值，$\mathbf{n}_g$ 和 $\mathbf{n}_a$ 分别是 $\omega_m$ 和 $\mathbf{a}_m$ 中的测量噪声，$\mathbf{b}_a$ 和 $\mathbf{b}_g$ 是IMU偏差，它们分别由高斯噪声 $\mathbf{n}_{b_g}$ 和 $\mathbf{n}_{b_a}$ 驱动的随机游走模型来描述。
### B. 扫描重组
我们采用扫描重组技术，将高频连续采样的激光雷达原始点云在相机采样时刻分割成独立的激光雷达扫描数据，如图3所示。这样就能确保相机和激光雷达数据以相同频率（例如10 Hz）同步，从而实现状态的同步更新。
---
```pseudo
// Algorithm 1: Sequential State Update.
```
1. 通过扫描重组使激光雷达数据与图像数据在相机采样率下同步；
2. 前向传播以获得状态预测 $\hat{\mathbf{x}}$ 及其协方差 $\hat{\mathbf{P}}$；
3. 反向传播用于对激光雷达点云进行运动补偿；
4. // 点对面激光雷达更新
5. $\kappa = -1$、$\hat{\mathbf{x}}^{\kappa=0} = \hat{\mathbf{x}}$；
6. 重复
7. $\kappa = \kappa + 1$；
8. 计算残差 $\mathbf{z}_l^{\kappa}$ 和雅可比矩阵 $\mathbf{H}_l^{\kappa}$；
9. 计算状态更新量 $\hat{\mathbf{x}}^{\kappa+1}$；
10. 直到 $\|\hat{\mathbf{x}}^{\kappa+1} \boxminus \hat{\mathbf{x}}^{\kappa}\| < \epsilon$；
11. $\hat{\mathbf{x}} = \hat{\mathbf{x}}^{\kappa+1}$、$\hat{\mathbf{P}} = (\mathbf{I} - \mathbf{K}\mathbf{H})\hat{\mathbf{P}}$；
12. // 稀疏直接视觉更新，层级 = -1；
13. 重复
14. $\kappa = -1$、$\hat{\mathbf{x}}^{\kappa=0} = \hat{\mathbf{x}}$；
15. 层级 = 层级 + 1；
16. 重复
17. $\kappa = \kappa + 1$；
18. 计算残差 $\mathbf{z}_c^{\kappa}$ 和雅可比矩阵 $\mathbf{H}_c^{\kappa}$；
19. 计算状态更新量 $\hat{\mathbf{x}}^{\kappa+1}$；
20. 直到 $\|\hat{\mathbf{x}}^{\kappa+1} \boxminus \hat{\mathbf{x}}^{\kappa}\| < \epsilon$；
21. $\hat{\mathbf{x}} = \hat{\mathbf{x}}^{\kappa+1}$；
22. 直到层级 ≥ 2；
23. $\bar{\mathbf{x}} = \hat{\mathbf{x}}^{\kappa+1}$；$\bar{\mathbf{P}} = (\mathbf{I} - \mathbf{K}\mathbf{H})\hat{\mathbf{P}}$。
### C. 状态传播
在ESIKF框架中，状态及其协方差会从接收到上一帧激光雷达扫描数据和图像帧的时间 $t_{k-1}$ 传播到接收到当前帧激光雷达扫描数据和图像帧的时间 $t_k$。这种前向传播会将过程噪声 $u_i$ 在公式(1)中设为零，从而预测 $t_{k-1}$ 和 $t_k$ 期间每个IMU输入时刻的状态 $w_i$。我们将传播后的状态记为 $\widehat{x}$，协方差记为 $\widehat{P}$，它们将作为第IV-D节中后续更新的先验分布。此外，为了补偿运动畸变，我们按照 [^42] 中的方法执行反向传播，以确保激光雷达扫描中的点在扫描结束时刻 $t_k$ 被“测量”。需要注意的是，为了简化表示，我们在所有状态向量中省略了下标k。
### D. 顺序更新
IMU传播后的状态 $\hat{x}$ 及其协方差 $\hat{P}$ 为时间 $t_k$ 的系统状态x提供了如下先验分布：
$$ \mathbf{x}\boxminus\widehat{\mathbf{x}}\sim\mathcal{N}\left(\mathbf{0},\widehat{\mathbf{P}}\right). $$
我们将上述先验分布记为 $p(\mathbf{x})$，激光雷达和相机的测量模型分别为
$$ \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} $$
其中，$\mathbf{v}_l \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_{\mathbf{v}_l})$ 和 $\mathbf{v}_c \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_{\mathbf{v}_c})$ 分别表示激光雷达和相机的测量噪声。
标准的ESIKF [^43]会利用所有当前测量值来更新状态x，包括激光雷达测量值$y_l$和图像测量值$y_c$。然而，激光雷达与图像测量属于两种不同的传感方式，其数据维度并不匹配。此外，图像测量的融合可能在图像金字塔的不同层级上进行。为解决这种维度不匹配问题并为各模块提供更大灵活性，我们提出了一种顺序更新策略。在假设激光雷达测量值$y_l$与图像测量值$y_c$在给定状态向量x的条件下统计独立（即测量值受统计独立的噪声干扰）时，该策略在理论上等同于利用所有测量值进行的标准更新。
为了引入这种顺序更新机制，我们将当前状态x的总条件分布重新表述为
$$ \begin{align*}p\left(\mathbf{x}|\mathbf{y}_{l},\mathbf{y}_{c}\right)&\propto p\left(\mathbf{x},\mathbf{y}_{l},\mathbf{y}_{c}\right)=p\left(\mathbf{y}_{c}|\mathbf{x},\mathbf{y}_{l}\right)p\left(\mathbf{x},\mathbf{y}_{l}\right)\\&=p\left(\mathbf{y}_{c}|\mathbf{x}\right)\underbrace{p\left(\mathbf{y}_{l}|\mathbf{x}\right)p\left(\mathbf{x}\right)}_{\propto p\left(\mathbf{x}|\mathbf{y}_{l}\right)}.\end{align*} $$
公式(5)表明，总条件分布$p(\mathbf{x} \mid \mathbf{y}_l, \mathbf{y}_c)$可以通过两次顺序贝叶斯更新得到。第一步仅将激光雷达测量值$\mathbf{y}_l$与IMU传播的先验分布$p(\mathbf{x})$进行融合，从而得到分布$p(\mathbf{x} \mid \mathbf{y}_l)$。
$$ p\left(\mathbf{x}|\mathbf{y}_{l}\right)\propto p\left(\mathbf{y}_{l}|\mathbf{x}\right)p\left(\mathbf{x}\right). $$
第二步则将相机测量值$\mathbf{y}_c$与$p(\mathbf{x} | \mathbf{y}_l)$融合，最终得到$\mathbf{x}$的后验分布。
$$ p\left(\mathbf{x}|\mathbf{y}_{l},\mathbf{y}_{c}\right)\propto p\left(\mathbf{y}_{c}|\mathbf{x}\right)p\left(\mathbf{x}|\mathbf{y}_{l}\right). $$
有趣的是，公式(6)和(7)中的两次融合形式相同，即$q\left(\mathbf{x}|\mathbf{y}\right)\propto q\left(\mathbf{y}|\mathbf{x}\right)q\left(\mathbf{x}\right)$。(8)
为了执行公式(8)中的融合操作，无论是针对激光雷达还是图像测量值，我们详细描述了先验分布$q(\mathbf{x})$和测量模型$q(\mathbf{y}|\mathbf{x})$。对于先验分布$q(\mathbf{x})$，将其记为$\mathbf{x} = \widehat{\mathbf{x}} \oplus \delta \mathbf{x}$，其中$\delta \mathbf{x} \sim \mathcal{N}(\mathbf{0}, \widehat{\mathbf{P}})$。在激光雷达更新阶段（即第一步），$(\widehat{\mathbf{x}}, \widehat{\mathbf{P}})$是传播步骤得到的状态与协方差；而在视觉更新阶段（即第二步），$(\widehat{\mathbf{x}}, \widehat{\mathbf{P}})$则是激光雷达更新后收敛的状态与协方差。
为了得到测量模型分布$q(\mathbf{y}|\mathbf{x})$，将第$\kappa$次迭代时估计的状态记为$\widehat{\mathbf{x}}^{\kappa}$，其中$\widehat{\mathbf{x}}^{0} = \widehat{\mathbf{x}}$。通过在$\widehat{\mathbf{x}}^{\kappa}$处对测量模型(4)（无论是激光雷达还是相机测量）进行一阶泰勒展开近似，可得
$$ \mathbf{y}\mid\mathbf{x}\simeq\underbrace{\mathbf{h}\left(\widehat{\mathbf{x}}^{\kappa},\mathbf{0}\right)}_{\mathbf{z}^{\kappa}}+\mathbf{H}^{\kappa}\delta\mathbf{x}^{\kappa}+\mathbf{L}^{\kappa}\mathbf{v} $$
$$ \begin{array}{r}{q(\mathbf{y}|\mathbf{x})\simeq\mathcal{N}\left(\mathbf{h}\left(\widehat{\mathbf{x}}^{\kappa},\mathbf{0}\right)+\mathbf{H}^{\kappa}\mathbf{\delta x}^{\kappa},\mathbf{R}\right)}\end{array} $$
其中$\delta \mathbf{x}^{\kappa} = \mathbf{x} \boxminus \widehat{\mathbf{x}}^{\kappa}$、$\mathbf{z}^{\kappa}$为残差，$\mathbf{L}^{\kappa} \mathbf{v} \sim \mathcal{N}(0, \mathbf{R})$为合并后的测量噪声，$\mathbf{H}^{\kappa}$和$\mathbf{L}^{\kappa}$分别是$\mathbf{h}(\widehat{\mathbf{x}}^{\kappa} \boxplus \delta \mathbf{x}^{\kappa}, \mathbf{v})$关于$\delta \mathbf{x}^{\kappa}$和$\mathbf{v}$的雅可比矩阵，均在零点处求值。
接着，将公式(10)中的先验分布$q(\mathbf{x})$和测量分布$q(\mathbf{y}|\mathbf{x})$代入后验分布(8)中，并执行最大似然估计，便可从ESIKF框架下的标准更新步骤中得到$\delta\mathbf{x}^{\kappa}$（进而得到$\mathbf{x}^{\kappa}$）的最大后验估计[^43]。
$$ \mathbf{K}=\left(\left(\mathbf{H}^{\kappa}\right)^{T}\mathbf{R}^{-1}\mathbf{H}^{\kappa}+\widehat{\mathbf{P}}^{-1}\right)^{-1}\left(\mathbf{H}^{\kappa}\right)^{T}\mathbf{R}^{-1} $$
$$ \mathbf{\widehat{x}}^{\kappa+1}=\mathbf{\widehat{x}}^{\kappa}{\boxplus}\left(-\mathbf{K}\mathbf{z}^{\kappa}-\left(\mathbf{I}-\mathbf{K}\mathbf{H}^{\kappa}\right)\left(\mathbf{\widehat{x}}^{\kappa}{\boxminus}\mathbf{\widehat{x}}\right)\right). $$
最终收敛的状态与协方差矩阵便构成了后验分布$q(\mathbf{x}|\mathbf{y})$的均值与协方差。
---
Fig. 4. 2-D demonstration of local map slide. In (a), the gray rectangle is the initial map region with length L. The red circle is the initial detection area centered at $p_{0}$. In (b), the detection area moves to a new position $p_{1}$ where the map boundaries are touched. The map region is moved to a new position (blue rectangle) by distance d. In (c), the memory space B remains unchanged. The memory space A storing the green area is reset for the blue area C in (b). (a) Initial map. (b) Move the map. (c) Reset the memory.
文献中已对采用顺序更新的卡尔曼滤波器展开研究，例如 [^44] 和 [^45]。本文针对激光雷达与摄像头系统采用 ESIKF 的这种方式。采用顺序更新的 ESIKF 的实现细节详见算法 1。第一步（第 6–10 行）中，根据激光雷达测量数据迭代更新误差状态（见第 VI-A 节），直至收敛。收敛后的状态与协方差估计值，再次记为 $\widehat{x}$ 和 $\widehat{P}$，用于更新地图的几何结构（见第 V-B 节）；随后在第二步视觉更新环节（第 13–23 行）中，在图像金字塔的各层级上进一步细化（见第 VII-B 节），直至收敛。最优状态与协方差记为 $\bar{x}$ 和 $\bar{P}$，用于传播传入的 IMU 测量数据（见第 IV-C 节）并更新地图的视觉结构（见第 V-D 节和第 V-E 节）。
### V. 局部建图
### A. 地图结构
我们的地图采用了 [^14] 中提出的自适应体素结构，该结构由哈希表以及每个哈希条目对应的八叉树组成（见图 2）。哈希表用于管理根体素，每个根体素的尺寸固定为 $0.5 \times 0.5 \times 0.5$ 米。每个根体素包含一个八叉树结构，用于进一步组织大小不一的叶体素。叶体素代表一个局部平面，存储平面特征（即平面中心、法向量及不确定性）以及位于该平面上的激光雷达原始点集。其中部分点会附带三级图像块（$8 \times 8$ 大小的块），我们将其称为视觉地图点。已收敛的视觉地图点仅附带参考块，而未收敛的点则同时附带参考块与其他可见块（见第 V-E 节）。叶体素的大小可变，因此能够表示不同尺度的局部平面，从而适配具有不同结构的环境 [^14]。
为防止地图规模无限制膨胀，我们仅保留激光雷达当前位置周围长度为 L 的大型局部区域内的局部地图，图 4 给出了二维示例。最初，地图是以激光雷达起始位置为中心的一个立方体 $p_0$。激光雷达的探测区域以当前位置为中心呈球形，其半径由激光雷达的探测范围决定。当激光雷达移动到新位置 $p_1$ 且探测区域触及地图边界时，我们会将地图向边界外移动距离 d。地图移动过程中，原本用于存储移出局部地图区域的数据的内存会被重置，以便存储移入局部地图的新区域。这种环形缓冲区机制确保了局部地图所占用的内存大小固定。环形缓冲区哈希表的实现细节详见 [^46]。每次 ESIKF 更新步骤之后都会执行地图移动检查。
### B. 几何构建与更新
地图的几何结构是根据激光雷达点测量数据构建并更新的。具体而言，在 ESIKF 完成激光雷达更新后（见第 IV 节），我们将激光雷达扫描中的所有点注册到全局坐标系下。对于每个已注册的激光雷达点，我们确定其在哈希表中对应的根体素。若该体素不存在，便用新点初始化该体素并将其索引到哈希表中；若确定的体素已在地图中存在，则将新点追加到现有体素中。扫描中的所有点分配完毕后，我们按如下方式开展几何构建与更新工作。
对于新创建的体素，我们会根据奇异值分解来判断其中所含的所有点是否位于同一平面上。如果是的话，我们便计算该平面的中心点$\mathbf{q} = \vec{\mathbf{p}}$、平面法向量n以及这些点的协方差矩阵$(\mathbf{q}, \mathbf{n})$，记为$\Sigma_{\mathbf{n}, \mathbf{q}}$。$\Sigma_{\mathbf{n}, \mathbf{q}}$用于表征平面的不确定性，这种不确定性源自位姿估计误差和点云测量噪声。关于平面的判定标准、平面参数及不确定性的计算方法，可参考我们之前的工作[^14]。如果所含点不在一个平面上，该体素会被继续细分为八个更小的八分体，直到确定子体素中的点能构成平面，或者达到最大细分层数（例如3层）为止。在后一种情况下，叶节点体素中的点将被丢弃。最终，地图中仅保留被识别为平面的体素（无论是根体素还是子体素）。
对于已有新点加入的现有体素，我们会判断这些新点是否与根体素或子体素中已有的点共同构成平面。如果不是，就按照前述方法对体素进行细分；如果是，我们也会像上面提到的那样更新平面的参数（q、n）以及协方差$\Sigma_{n,q}$。一旦平面参数收敛（详见[^14]），该平面就会被认定为成熟平面，此后落在该平面上的新点将被丢弃。此外，成熟平面的估计平面参数（q、n）和协方差$\Sigma_{n,q}$也会被固定下来。
位于平面上的激光雷达点（无论处于根体素还是子体素中）会被用于后续生成视觉地图点。对于成熟平面，最近的50个激光雷达点可作为视觉地图点的候选点；而对于非成熟平面，所有激光雷达点均为候选点。视觉地图点的生成过程会从这些候选点中挑选出部分作为视觉地图点，并为它们附加图像块以实现图像对齐。
### C. 视觉地图点的生成与更新
为了生成和更新视觉地图点，我们会从地图中选出满足以下条件的候选激光雷达点：1）能从当前帧中看到（详见第VII-A节）；2）在当前图像中呈现明显的灰度梯度。在视觉更新之后（见第IV-D节），我们将这些候选点投影到当前图像上，并在每个体素中保留该局部平面上深度最小的候选点。接着，我们将其划分
---
为若干个大小均为$30 \times 30$像素的统一网格单元。如果某个网格单元中没有投影在其上的视觉地图点，我们就利用灰度梯度最高的候选点来生成新的视觉地图点，并为其关联当前图像块、估计的当前状态（即帧位姿和曝光时间），以及上一节中根据激光雷达点计算出的平面法向量。附着在视觉地图点上的图像块包含三层大小相同的像素数据（例如$11 \times 11$像素），每一层都是对上一层进行半采样得到的，从而构成图像块金字塔。如果某个网格单元中已有投影在其上的视觉地图点，那么当满足以下任一条件时，我们就会为现有的视觉地图点添加新的图像块（金字塔的三层）：1）自上次添加图像块以来已经过去了20帧以上；2）该视觉地图点在当前帧中的像素位置与其上次添加图像块时的位置偏差超过40像素。如此一来，地图点就能拥有视角分布均匀的有效图像块。除了图像块金字塔外，我们还会将估计的当前状态（即位姿和曝光时间）附加到地图点上。
### D. 参考图像块更新
由于新补丁的加入，一个视觉地图点可能会包含多个补丁。在视觉更新中，我们需要为图像对齐选择一个参考补丁。具体来说，我们会根据光度相似性和视角对各个补丁 f 进行评分：
$$ \begin{aligned}NCC\left(\mathbf{f},\mathbf{g}\right)&=\frac{\sum_{x,y}\left[\mathbf{f}(x,y)-\mathbf{f}\right]\left[\mathbf{g}(x,y)-\overline{\mathbf{g}}\right]}{\sqrt{\sum_{x,y}\left[\mathbf{f}(x,y)-\overline{\mathbf{f}}\right]^{2}\sum_{x,y}\left[\mathbf{g}(x,y)-\overline{\mathbf{g}}\right]^{2}}}\\c&=\frac{\mathbf{n}\cdot\mathbf{p}}{\left\|\mathbf{p}\right\|},\quad\omega_{1}=\frac{1}{1+e^{\mathrm{tr}\left(\mathbf{\Sigma _{n}}\right)}}\\S&=\left(1-\omega_{1}\right)\cdot\frac{1}{n}\sum_{i=1}^{n}NCC\left(\mathbf{f},\mathbf{g}_{i}\right)+\omega_{1}\cdot c\quad(12)\end{aligned} $$
其中，$\text{NCC}(f, g)$ 表示归一化互相关（NCC），用于衡量补丁 f 与 g 在第 0 层金字塔级别（分辨率最高的层级）上的相似性；计算时会对两个补丁都进行均值减除处理。c 表示法向量 n 与点 p 或待评估补丁 f 中的 $\left\| p \right\|$ 之间的余弦相似度；当补丁正对着地图点所在的平面时，c 的值为 1。最终得分 S 是加权后的 NCC 与 c 之和，其中前者代表待评估补丁 f 与其他所有补丁之间的平均相似性，$g_i$ 和 $\text{tr}(\Sigma_n)$ 则表示法向量协方差矩阵的迹。
在所有附着于某个视觉地图点的补丁中，得分最高的补丁会被更新为参考补丁。上述评分机制倾向于选择这样的参考补丁：1）其外观（就 NCC 而言）与大多数其他补丁相似，MVS [^47] 采用这种方法来避开动态物体上的补丁；2）其观测方向垂直于平面，从而能保留高分辨率的纹理细节。相比之下，我们之前的工作 FAST-LIVO [^8] 以及现有方法 [^4] 中的参考补丁更新策略是直接选择观测方向与当前帧差异最小的补丁，这会导致所选参考补丁与当前帧极为接近，进而对当前位姿更新的约束较弱。
Fig. 5. (a) Affine warping between reference patches and target patches. (b) Any normal $I_r \mathbf{n} \in \mathbb{S}^2$ lying on the normalized sphere is first projected into a point $\mathbf{M} \in \mathbb{R}^3$ on the plane $I_r \mathbf{p}^T \mathbf{M} = 1$, and then projected to a point $\mathbf{m} \in \mathbb{R}^2$ on the x-y plane. This transformation thereby converts a perturbation $\delta \mathbf{n}$ on the sphere into a perturbation $\delta \mathbf{m}$ in x-y plane.
### E. 法向量优化
每个视觉地图点都被假定位于一个小平面上。现有研究 [^2]、[^4]、[^8] 假设补丁内的所有像素深度相同，这一假设通常并不成立。我们利用第 V-B 节详述的、由激光雷达点计算得到的平面参数来提升精度。该平面法向量对于视觉更新过程中执行图像对齐的仿射变换至关重要。为了进一步提高仿射变换的精度，还可以根据附着于视觉地图点的补丁进一步优化平面法向量。具体而言，我们通过最小化参考补丁与其他附着于该视觉地图点的补丁之间的光度误差，来对参考补丁中的平面法向量进行优化。
1) 仿射变换：仿射变换用于将参考帧中的补丁像素（即源补丁）转换为其他帧中的补丁像素（即目标补丁），如图 5(a) 所示。设 $\mathbf{u}_r^j$ 为源补丁中第 j 个像素的坐标，$\mathbf{u}_i^j$ 为第 i 个目标补丁中第 j 个像素的坐标。假设补丁内的所有像素都位于以 $l_r^n$ 为法向量、视觉地图点位置为 $l_r^p$（对应源补丁和目标补丁的中心像素）的平面上，且这些量均以源补丁坐标系表示，则有
$$ \begin{aligned}\mathbf{u}_{i}^{j}&=\mathbf{A}_{r}^{i}\mathbf{u}_{r}^{j}\\\mathbf{A}_{r}^{i}&=\mathbf{P}\left(^{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}}\right)\mathbf{P}^{-1}\end{aligned} $$
其中，$\mathbf{A}_r^i$ 表示仿射变换矩阵，它可将像素坐标从源（或参考）图像块转换到第 i 个目标图像块；$I_r\mathbf{R}_{I_r}$ 和 $I_i\mathbf{t}_{I_r}$ 分别表示参考坐标系 $I_r$ 相对于目标坐标系 $I_i$ 的相对位姿。为了直接使用鱼眼图像而无需将其校正为针孔图像，我们基于不同的相机模型实现了投影矩阵 $\mathbf{P}$ 和逆投影矩阵 $\mathbf{P}^{-1}$（例如，$\mathbf{P}$ 就是针孔相机模型的相机内参矩阵）。
2) 法线优化：为了精细化平面法线 $I_r n$，我们最小化了第 0 层金字塔（即最高分辨率层）中参考图像块与其他图像块之间的光度误差。
$$ ^{I_{r}}\mathbf{n}^{*}=\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}\left(\mathbf{A}_{r}^{i}\mathbf{u}_{r}^{j}\right)-\tau_{r}\mathbf{I}_{r}\left(\mathbf{u}_{r}^{j}\right)\right\|_{2} $$
---
其中，N 表示路径长度，$\tau_r$ 和 $\tau_i$ 分别是参考帧和第 i 个目标帧的逆曝光时间。$\mathbf{I}_r(\mathbf{u}_r^j)$ 表示参考帧中的第 j 个图像块像素，$\mathbf{I}_i(\mathbf{A}_r^iu_r^j)$ 表示第 i 个目标帧中的第 j 个路径像素，S 则是所有目标帧的集合。
3) 优化变量变换：为了提升计算效率，我们对式(14)中的最小二乘问题进行重新参数化。注意，优化变量 $^{I_r}\mathbf{n}$ 仅出现在式(13)中的 $\mathbf{M} \triangleq \frac{1}{r_r \mathbf{n}^{T, I_r} \mathbf{p}} I_r \mathbf{n} \in \mathbb{R}^3$ 里，因此可以对 $^{I_r}\mathbf{n}$ 进行优化的范围扩展至 $\mathbf{M}$。此外，向量 $\mathbf{M}$ 需满足约束 $^{I_r}\mathbf{p} \cdot \mathbf{M} = 1$，这意味着 $\mathbf{M}$ 可按照如下方式参数化：
$$ \mathbf{M}=\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} $$
$$ \mathbf{B}=\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} $$
其中 $^I_r\mathbf{p}_z \neq 0$，因为无法为该视觉地图点选取这样的参考图像块。$^I_r\mathbf{n}$、M 和 m 之间的关系如图 5(b)所示。
最终，式(14)中的优化是针对向量 $\mathbf{m} \in \mathbb{R}^2$ 进行的，且没有任何约束。这项优化可在独立线程中执行，以免阻塞主里程计线程。优化得到的参数 $\mathbf{m}^*$ 随后可用于恢复最优法线向量 $\mathbf{I}_r \mathbf{n}^*$。
$$ {}^{I_{r}}\mathbf{n}^{*}=\frac{\mathbf{M}^{*}}{\left\|\mathbf{M}^{*}\right\|},\mathbf{M}^{*}=\mathbf{B}\mathbf{m}^{*}+\mathbf{b}. $$
一旦平面法线收敛，该视觉地图点的参考图像块和法线向量就会被固定，不再进一步细化，同时会删除所有其他图像块。
### 六、激光雷达测量模型
本节详细介绍了第四节 D 部分 ESIKF 中激光雷达更新所使用的激光雷达测量模型 $\mathbf{y}_l = \mathbf{h}_l(\mathbf{x}, \mathbf{v}_l)$。
### A. 点到平面激光雷达测量模型
在扫描中获取无畸变点 $\{^L p_j\}$ 后，我们利用激光雷达更新过程中第 $\hat{\mathbf{x}}^{\kappa}$ 次迭代时估计的状态 $\kappa$，将这些点投影到全局坐标系下。
$$ {}^{G}\widehat{\mathbf{p}}_{j}^{\kappa}={}^{G}\widehat{\mathbf{T}}_{I}^{\kappa I}\mathbf{T}_{L}{}^{L}\mathbf{p}_{j}. $$
接着，我们确定 $G\widehat{\mathbf{p}}_j^\kappa$ 在哈希表中对应的体素或子体素。如果未找到对应体素，或者该体素中不包含平面，则该点会被丢弃。否则，我们利用该体素中的平面来建立激光雷达点的测量方程。具体来说，我们假设在精确的激光雷达位姿 $L\mathbf{p}_j^{gt}$ 下，真实的激光雷达点 $G\mathbf{T}_I$ 应位于该体素中法线为 $\mathbf{n}_j^{gt}$、中心点坐标为 $\mathbf{q}_j^{gt}$ 的平面上，即：
$$ \mathbf{0}=\left(\mathbf{n}_{j}^{g t}\right)^{T}\left({^{G}\mathbf{T}_{I}}{^{I}\mathbf{T}_{L}}{^{L}\mathbf{p}_{j}^{g t}}-\mathbf{q}_{j}^{g t}\right). $$
由于真实点 $^L\mathbf{p}_j^{gt}$ 的测量结果为 $^L\mathbf{p}_j$，且带有测距和方位噪声 $\delta^L\mathbf{p}_j$，因此有 $^L\mathbf{p}_j^{gt} = ^L\mathbf{p}_j - \delta^L\mathbf{p}_j$。同理，平面参数 $(\mathbf{n}_j^{gt}, \mathbf{q}_j^{gt})$ 的估计结果为：
(a)
Fig. 6. (a) and (b), respectively, illustrate the 3-D and side crosssectional views of the LiDAR point uncertainty model considering the laser beam divergence angle $\theta$. The red contour outlines the area that a laser beam spreads. (c) and (d) color the points in a scan by the point location uncertainty. Compared to (c), (d) further takes into account the ranging uncertainty $\delta d$ resulting from the beam divergence angle. This leads to a higher uncertainty for ground points due to the large spread area of laser beams.
$(n_j, q_j)$ 与协方差 $\Sigma_{n,q}$ 相关（详见第 V-B 节），因此我们有：
$\mathbf{n}_j^{gt} = \mathbf{n}_j \boxminus \delta \mathbf{n}_j$、$\mathbf{q}_j^{gt} = \mathbf{q}_j - \delta \mathbf{q}_j$。因此
$$ \underbrace{\mathbf{0}}_{\mathbf{y}_{l}}=\underbrace{(\mathbf{n}_{j}\boxminus\delta\mathbf{n}_{j})^{T}\left(^{G}\mathbf{T}_{I}{}^{I}\mathbf{T}_{L}\left({}^{L}\mathbf{p}_{j}-\delta^{L}\mathbf{p}_{j}\right)-(\mathbf{q}_{j}-\delta\mathbf{q}_{j})\right)}_{\mathbf{h}_{l}(\mathbf{x},\mathbf{y}_{l})} $$
其中测量噪声 $\mathbf{v}_l = (\delta^L \mathbf{p}_j, \delta \mathbf{n}_j, \delta \mathbf{q}_j)$ 分别由激光雷达点、法向量和平面中心相关的噪声构成。
### B. 考虑光束发散的激光雷达测量噪声
激光雷达点在局部激光雷达坐标系中的不确定性 $\delta^L p_j$ 在 [^14] 中被分解为两个部分：由激光飞行时间（TOF）引起的测距不确定性 $\delta d$，以及源自编码器的方位角不确定性 $\delta \omega$。除了这些不确定性外，我们还会考虑由激光束发散角 $\theta$ 引起的不确定性，如图 6 所示。随着方位角与法向量之间的夹角 $\varphi$ 增大，激光雷达点的测距不确定性会显著增加，而方位角不确定性则不受影响。由激光束发散角引起的 $\delta d$ 可建模为
$$ \delta d=L_{2}-L_{1}=d\biggl(\frac{\cos\varphi}{\cos(\theta+\varphi)}-\frac{\cos\varphi}{\cos(\theta-\varphi)}\biggr). $$
考虑到受 TOF 和激光束发散影响的 $\delta d$，当系统从地面或墙壁上选取更多点时[见图 6(c) 和 (d)]，其获得的位姿估计精度要高于未考虑该效应的情形。
### VII. 视觉测量模型
本节详细介绍第 IV-D 节中 ESIKF 视觉更新所使用的视觉测量模型 $\mathbf{y}_c = \mathbf{h}_c(\mathbf{x}, \mathbf{v}_c)$。
---
Fig. 7. Illustration of on-demand voxel raycasting.
### A. 视觉地图点选择
为了在视觉更新中执行稀疏图像对齐，我们首先选择合适的视觉地图点。首先，利用体素查询和光线投射查询，提取当前相机视场中可见的地图点集合（称为视觉子图）。接着从该子图中筛选出视觉地图点并剔除异常值。这一过程最终得到一组经过优化的视觉地图点，可用于在视觉测量模型中构建视觉光度误差。
1) 可见体素查询：由于地图中体素数量庞大，识别当前帧视场内的地图体素颇具挑战性。为解决这一问题，我们查询当前扫描中激光雷达点所击中的体素。通过用测量到的点位置查询体素哈希表，即可高效完成此操作。如果相机视场与激光雷达视场有大量重叠，那么相机视场内的地图点很可能也位于这些体素中。我们还会查询上一图像帧中经确认可见的地图点所击中的体素（同样通过体素查询和光线投射实现），因为连续两帧图像的视场重叠度较高。最终，通过这两类体素中包含的地图点并经视场检查，即可得到当前的视觉子图。
2) 按需光线投射：在大多数情况下，可以通过上述的体素查询获取视觉子地图。然而，当激光雷达传感器距离物体过近时，可能无法返回任何点（即所谓的近距离盲区）。此外，相机的视场可能无法被激光雷达的视场完全覆盖。为了在这些情况下获取更多视觉地图点，我们采用了光线投射策略，如图7所示。我们将图像划分为均匀的网格单元，每个单元包含$30 \times 30$个像素，并将通过体素查询得到的视觉地图点投影到这些网格单元上。对于未被视觉地图点占据的每个图像网格单元，会沿中心像素向后发射一条光线，在深度方向上从$d_{min}$到$d_{max}$均匀分布采样点。为降低计算量，相机坐标系下每条光线上的采样点位置会被预先计算好。对于每个采样点，我们会评估对应体素的状态：如果该体素中包含投影后位于该网格单元内的地图点，就将这些地图点纳入视觉子地图，并停止对该光线的处理；否则，继续沿光线查找下一个采样点，直到达到最大深度$d_{max}$。在处理完所有未被占据的图像网格单元后
(a)
Fig. 8. Outlier rejection. (a) shows the diagrammatic drawing of occluded and depth-discontinuous visual map points. (b) shows the effect of outlier rejection in real scenes. The red dots are the rejected visual map points, and the green dots are the accepted visual map points.
通过光线投射，我们就能得到散布在整个图像中的一组视觉地图点。
3) 异常点剔除：经过体素查询和光线投射后，我们获得了当前帧视场内的所有视觉地图点。然而，这些视觉地图点可能在当前帧中被遮挡、存在深度不连续现象、参考图像块是在较大视角下采集的，或者当前帧中的视角过大，这些因素都会严重降低图像对齐的精度。为解决第一个问题，我们利用激光雷达更新后的位姿，将子地图中的所有视觉地图点投影到当前帧中，并保留每个大小为$30 \times 30$像素的网格单元内深度最小的点。为解决第二个问题，我们将当前激光雷达扫描得到的点投影到当前帧中，生成深度图。通过比较视觉地图点的深度与其在深度图中$9 \times 9$邻域内的深度，就能判断其是否被遮挡以及是否存在深度变化。被遮挡或深度不连续的地图点会被剔除（见图8）。为解决第三和第四个问题，我们会移除那些参考图像块或当前图像块的视角（即法向量与从视觉地图点到图像块光学中心的方向之间的夹角）过大的点（例如超过$80^{\circ}$）。剩余的视觉地图点将被用于对齐当前图像。
### B. 稀疏直接视觉测量模型
先前提取的视觉地图点$\{G_{\mathbf{p}_i}\}$被用于构建视觉测量模型。其基本原理是：当把地图点$G_{\mathbf{p}_i}$利用真实状态（即位姿）$\mathbf{I}_k(\cdot)$变换到当前图像$\mathbf{x}_k$上时，参考图像块与当前图像块之间的光度误差应当为零。
$$ \begin{aligned}\mathbf{0}&=\tau_{k}\mathbf{I}_{k}^{gt}(\underbrace{\boldsymbol{\pi}\left(^{C}\mathbf{T}_{I}(^{G}\mathbf{T}_{I})^{-1G}\mathbf{p}_{i}\right)}_{\mathbf{u}_{i}}+\Delta\mathbf{u})\\&\quad-\tau_{r}\mathbf{I}_{r}^{gt}(\underbrace{\boldsymbol{\pi}\left(^{C_{r}}\mathbf{T}_{G}{}^{G}\mathbf{p}_{i}\right)}_{\mathbf{u}_{i}^{\prime}}+\mathbf{A}_{i}^{r}\Delta\mathbf{u})\end{aligned} $$
其中，$\pi(\cdot)$ 是常用的相机投影模型（即针孔模型、MEI模型、ATAN模型、Scaramuzza模型、等距投影模型），$C_rT_G$ 是全局坐标系G相对于参考坐标系$C_r$的位姿，该位姿是在接收并融合参考坐标系时估计得到的；$\mathbf{A}_i^r$ 是将第i个当前图像块中的像素变换到参考图像块的仿射变换矩阵，$\Delta\mathbf{u}$ 是当前图像块内相对于中心点$\mathbf{u}_i$的相对像素位置，$\mathbf{I}_k^{gt}$ 和 $\mathbf{I}_r^{gt}$ 分别表示参考帧和当前帧的真实像素值。
---
它们分别被测量为实际的图像像素值$\mathbf{I}_k$、$\mathbf{I}_r$，且带有测量噪声$\mathbf{v}_c = (\delta \mathbf{I}_k, \delta \mathbf{I}_r)$，这些噪声源自多种来源[例如相机的CMOS传感器的散粒噪声和模数转换器(ADC)噪声]。因此
$$ \underbrace{\mathbf{0}}_{\mathbf{y}_{c}}=\underbrace{\tau_{k}\left(\mathbf{I}_{k}\left(\mathbf{u}_{i}+\Delta\mathbf{u}\right)-\delta\mathbf{I}_{k}\right)-\tau_{r}\left(\mathbf{I}_{r}(\mathbf{u}_{i}^{\prime}+\mathbf{A}_{i}^{r}\Delta\mathbf{u})-\delta\mathbf{I}_{r}\right)}_{\mathbf{h}_{c}(\mathbf{x},\mathbf{v}_{c})}. $$
为了提升计算效率，我们采用了逆合成公式[^4]、[^48]，其中用于参数化$\delta\mathbf{T} \in \mathbb{R}^6$中$G\mathbf{T}_I = G\widehat{\mathbf{T}}_I^\kappa \exp(\delta\mathbf{T})$的位姿增量$\mathbf{u}_i$[见式(21)]，会被从$\mathbf{u}_i$移至$\mathbf{u}_i'$，具体过程如下：
$$ \begin{aligned}\mathbf{u}_{i}&=\boldsymbol{\pi}\left({^{C}\mathbf{T}_{I}}\left({^{G}\widehat{\mathbf{T}}_{I}^{\kappa}}\right)^{-1}{^{G}\mathbf{p}_{i}}\right)\\\mathbf{u}_{i}^{\prime}&=\boldsymbol{\pi}\left({^{C_{r}\mathbf{T}_{G}}\mathrm{E x p}\left(\delta\mathbf{T}\right)^{G}\mathbf{p}_{i}}\right).\end{aligned} $$
由于每次迭代过程中参考坐标系下的$\mathbf{u}_i'$保持不变，因此我们只需一次性计算关于$\delta\mathbf{T}$的雅可比矩阵，无需在每次迭代时重新计算。
为了从式(22)的测量结果中估计逆曝光时间$\tau_k$，我们固定初始逆曝光时间$\tau_0 = 1$，以避免当所有逆曝光时间均为零时式(22)出现退化现象。因此，后续帧的估计逆曝光时间实际上是相对于第一帧的曝光时间。
式(22)在三个层级上的视觉更新步骤中被使用（详见算法1）；视觉更新从最粗糙的层级开始，某一层级收敛后便进入下一个更精细的层级。随后，估计得到的状态会被用来生成视觉地图点（详见第V-C节）并更新参考图像块（详见第V-D节）。
### 八、用于评估的数据集
本节将介绍用于性能评估的数据集，包括公开数据集NTU-VIRAL [^49]、Hiti'22 [^50]、Hiti'23 [^51]、MARS-LVIG [^52]，以及我们自行采集的FAST-LIVO2私有数据集。具体而言，NTU-VIRAL和Hiti数据集用于对我们的系统与其他前沿SLAM系统进行定量基准对比（详见第IX-B节）。FAST-LIVO2私有数据集主要用于在各种极具挑战性的场景下评估我们的系统（详见第IX-C节），展示其高精度建图能力（详见第IX-D节），并验证系统内各模块的功能（详见补充材料中的第I-A至I-D节 [^53]）。MARS-LVIG数据集则用于应用演示（详见第X节）和消融研究（详见补充材料中的第I-E节 [^53]）。
### A. NTU-VIRAL、Hiti和MARS-LVIG数据集
NTU-VIRAL数据集是在南洋理工大学校园内借助空中平台采集的，涵盖了多种体现独特空中作业挑战的场景。具体而言，“sbs”序列只能提供来自远距离物体的含噪视觉特征。“nya”序列由于存在半透明表面，会给激光雷达SLAM带来挑战；而复杂的飞行动力学和低光照条件则对视觉SLAM构成挑战。该数据集配备了一台采样率为10 Hz的16通道OS1 gen1 $^{3}$激光雷达，以及采样率为100 Hz的内置惯性测量单元（IMU），还有两台同步触发的针孔相机，触发频率为10 Hz。左侧相机用于评估。
Hilt $^{1}$22和Hilt $^{1}$23数据集是由手持设备和机器人设备采集的，包含来自建筑工地、办公室、实验室和停车场等环境的室内外序列。这些序列带来了诸多挑战，比如长走廊、地下室和楼梯环境，无纹理特征、光照条件多变以及激光雷达平面约束不足等问题。手持设备采集的序列使用了采样率为10 Hz的禾赛科技PandarXT-32 $^{4}$激光雷达，以及采样率为40 Hz的五台广角相机（随后降采样至10 Hz），还有采样率为400 Hz的外部博世BMI085惯性测量单元（IMU）。而机器人搭载的序列则配备了采样率为10 Hz的Robosense BPearl $^{5}$激光雷达、采样率为10 Hz的八台全向相机，以及采样率为200 Hz的Xsens MTi-670惯性测量单元（IMU）。在这两种情况下，前置相机均用于所有待评估系统。每个序列都提供了毫米级精度的真实值数据，这些数据是通过动作捕捉系统或全站仪 [^54] 获取的。需要注意的是，Hilt $_1$数据集的真实值数据并未开源；因此，在这些数据集上的算法结果需通过Hilt $^{1}$官方网站进行评估。由于Hilt $^{1}$23中的“Site 3”序列未提供详细的分析图表（如均方根误差），我们排除了这四个序列，但针对这些序列的评分结果仍可在其官方网站上查看。$^{6}$ NTU-VIRAL和Hilt $^{1}$数据集共包含25个序列。
MARS-LVIG数据集提供了高空面向地面的测绘数据，涵盖丛林、山脉和岛屿等多种非结构化地形。该数据集是通过大疆M300 RTK四旋翼无人机采集的，机上配备了Livox Avia $^{7}$激光雷达（内置BMI088惯性测量单元）和高分辨率全局快门相机，两者触发频率均为10 Hz。这与前述的NTU-VIRAL和Hilti数据集有明显区别：后者使用的是$752 \times 480$灰度图像，而MARS数据集则采用$2448 \times 2048$ RGB图像，从而能够生成清晰、密集的彩色点云。因此，我们利用这一公开数据集来验证我们在高空航空测绘应用方面的能力。
### B. FAST-LIVO2私有数据集
为了验证该系统在更极端条件下的性能（如激光雷达退化、低光照、剧烈曝光变化以及无激光雷达测量的情况），我们制作了一个名为FAST-LIVO2私有数据集的新数据集。该数据集、硬件设备以及硬件同步方案会与本研究的代码一同发布，以便于他人复现我们的研究成果。
1) 平台：我们的数据采集平台如图9所示，配备了工业相机（MV-CA013-21UC）、Livox Avia激光雷达，以及作为机载计算机的DJI Manifold-2c（英特尔i7-8550u处理器，8GB内存）。相机的视场角为$70.6^{\circ} \times 68.5^{\circ}$，激光雷达的视场角为$70.4^{\circ} \times 77.2^{\circ}$。所有传感器均由STM32同步定时器生成的10 Hz触发信号进行硬件同步。
2) 序列描述：如补充材料中的表S1[^53]所总结的，FAST-LIVO2数据集
---
Fig. 9. Our platform with hardware synchronization for data acquisition. (a) Our handheld platform. (b) Hardware synchronization scheme.
该数据集包含20个来自不同场景的序列（例如校园建筑、走廊、地下室、采矿隧道等），这些场景具有无结构、杂乱、光线昏暗、光照变化大且纹理较弱的特点，总时长为66.9分钟。大多数序列都存在视觉和/或激光雷达数据退化现象，比如面对单一且无纹理的平面、穿越极其狭窄且黑暗的隧道，以及经历从室内到室外的光照条件变化（详见补充材料中的图S7 [^53]）。为确保相机与激光雷达之间的数据同步采集效果更佳，我们在大多数场景下将相机设置为固定曝光时间但开启自动增益模式。对于采用自动曝光模式的其余序列，我们会记录其真实的曝光时间。所有序列中，平台都会返回起点，这为漂移评估提供了便利。
### 九、实验结果
在本节中，我们开展了大量实验来评估所提出的系统。
### A. 实现方式与系统配置
我们采用C++和机器人操作系统实现了所提出的FAST-LIVO2系统。在默认配置下，曝光时间估计功能处于启用状态，而法向量优化功能则被关闭。一次扫描中的激光雷达点会按1:3的比例进行时间上的降采样处理。体素地图的根体素大小被设定为0.5米，内部八叉树的最大层数设为3。用于图像对齐的图像块大小为$8 \times 8$，用于法向量优化的图像块大小为$11 \times 11$。在顺序ESIKF设置中，对于所有实验，相机的光度噪声被设定为恒定值100。Livox Avia激光雷达和OS1-16的激光雷达深度误差与方位角误差分别调整为0.02米和$0.05^\circ$；PandarXT-32的为0.001米和$0.001^\circ$；Robosense BPearl激光雷达的为0.008米和$0.01^\circ$。Livox Avia激光雷达和OS1-16的激光束发散角被设定为$0.15^\circ$，PandarXT-32和Robosense BPearl激光雷达的则为$0.001^\circ$。在所有采用相同传感器配置的数据集序列中，我们的系统均使用相同的参数。所有实验的计算平台均为配备Intel i7-10700K CPU和32GB RAM的台式电脑。对于FAST-LIVO2，我们也在嵌入式系统中常用的、功耗和成本较低的ARM处理器上进行了测试。该ARM平台为RB5 $^8$，搭载Qualcomm Kryo585 CPU和8GB RAM。我们将FAST-LIVO2在基于ARM的平台上的实现称为“FAST-LIVO2 (ARM)”。
### B. 基准测试实验
在本实验中，我们对来自NTU-VIRAL、Hilti'22以及23个公开数据集的25个序列进行了定量评估。我们的方法会与几种最先进的开源里程计系统进行对比测试，这些系统包括：R3LIVE [^36]，一种密集直接式LIVO系统；FAST-LIO2 [^13]，一种直接式激光雷达-惯性里程计系统；SDV-LOAM [^40]，一种半直接式激光雷达-视觉里程计系统；LVI-SAM [^35]，一种基于特征的激光雷达-惯性-视觉SLAM系统；以及我们之前的工作FAST-LIVO [^8]。
这些系统是从各自的GitHub仓库下载的。对于FAST-LIO2、FAST-LIVO和LVI-SAM，我们采用了针对配备多线激光雷达传感器的室内外场景的推荐配置。对于R3LIVE，我们对其进行了适配，使其能支持鱼眼相机模型以及搭载外部IMU的多线激光雷达（默认配置仅支持内部IMU）。由于数据集中IMU激励不足会导致优化效果不佳，我们关闭了相机内参和外参的实时优化功能$^C T_I$。其他参数，包括光流跟踪的窗口大小与金字塔层级、当前扫描点云及全局地图的降采样分辨率，都经过了精细调整以实现最佳性能。由于只有SDV-LOAM的视觉模块开源，我们按照原文中的方法[^7]，将其与LeGO-LOAM以松耦合方式集成[^40]。这一改进后的系统会持续优化从视觉模块获取的位姿，我们也已将相关实现代码开源$^9$。鉴于除LVI-SAM外，所有对比系统均为无回环闭合功能的里程计，为确保公平比较，我们移除了LVI-SAM的回环闭合模块。此外，我们还对曝光时间估计模块、法线细化模块以及参考块更新策略进行了消融研究。默认的FAST-LIVO2具备实时曝光估计和参考块更新功能，但没有法线细化功能。
表二展示了所有方法的结果。可以看出，我们的方法在所有序列中均取得了最高的整体精度，平均RMSE为0.044米，这一数值比排名第二的FAST-LIVO（0.137米）高出三倍。除“室外建筑”和“大房间（黑暗）”这两个序列外，我们的系统在大多数序列中均取得了最佳结果；在这两个序列中，我们的系统产生的误差略高于仅使用激光雷达与惯性传感器的里程计FAST-LIO2，误差水平仅在毫米级。这种差异源于这些序列结构特征丰富但光照条件较差，导致图像昏暗且模糊。因此，融合这类低质量图像并不能提升里程计精度。排除这两个序列后，我们这种充分利用激光雷达、惯性传感器和视觉信息紧密耦合的方法，其性能显著优于FAST-LIO2、我们的LIO子系统以及仅依赖激光雷达与视觉信息的里程计SDV-LOAM。值得注意的是，由于缺乏与IMU测量的紧密集成，SDV-LOAM在Hilti数据集上的表现尤其差，这导致了LO子系统的漂移现象。此外，激光雷达与视觉观测之间的松耦合，加上初始值不佳……
---
TABLE II ABSOLUTE TRANSLATIONAL ERRORS (RMSE, METERS) IN SEQUENCES
| 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 |
| Hiti'22 | Construction ground | 25.121 | 0.011 | 0.013 | 0.021 | $\times$ | 0.022 | 0.011 | 0.008 | 0.015 | 0.010 |
| Construction multilevel | 12.561 | 0.031 | 0.044 | 0.024 | $\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 | $\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 | $\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 |
| Hiti'23 | Floor 0 | 4.621 | 0.028 | 0.031 | 0.022 | $\times$ | 0.021 | 0.025 | 0.023 | 0.023 | 0.018 |
| 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 | $\times$ | 0.048 | 0.023 | 0.021 | 0.051 | 0.022 |
| Basement | 6.151 | 0.021 | 0.038 | 0.024 | $\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 | $\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 | $\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 |
×表示系统完全失效。
VO往往会导致局部最优解，甚至产生负优化现象。由于我们对每个激光雷达点采用了更精确的噪声建模，我们的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中光线投射技术的有效性；这些大规模场景的建图结果详见补充材料中的图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数据集上开展了深入研究。受篇幅限制，相关结果详见补充材料中的第I节（系统模块验证）[^53]。研究结果证实：无论是在结构化还是非结构化环境中，无论光照变化多么剧烈，无论是在规模极大的场景下进行的长期高速数据采集过程中，还是在激光雷达测量数据极少的极端狭窄空间中，我们的系统都能实现鲁棒且精准的位姿估计。
### C. 激光雷达退化及视觉挑战环境
在本实验中，我们评估了系统在激光雷达退化和/或视觉挑战环境下的鲁棒性，并与FAST-LIVO、R3LIVE在八个序列中的定性建图结果进行了比较，具体如下所示。
---
Fig. 10. Mapping results generated in real time in LiDAR degenerated scenes. The point clouds from top to bottom correspond to "Bright Screen Wall," "Black Screen Wall," "HIT Graffiti Wall" (the third and fourth rows), "Banner Wall," "HKU Lecture Center," respectively, showing the comparison of colored point cloud constructed FAST-LIVO2, FAST-LIVO, or R3LIVE (see more details on YouTube). $^{10}$
如图10和图11所示。图10展示了激光雷达退化序列：激光雷达沿墙面从一侧移动到另一侧，且始终面对着一面大墙。由于激光雷达只能观测到一个墙面平面，缺乏几何约束，因此LIO方法会失效。值得一提的是，“HIT涂鸦墙”序列中激光雷达连续面向墙面行进近800米，导致明显的退化现象。在所有序列中，FAST-LIVO2均展现出其对长期退化情况的强大鲁棒性，以及生成高精度彩色点云地图的能力。相比之下，FAST-LIVO虽然能获取几何结构，但纹理完全模糊；R3LIVE在几何结构和纹理清晰度方面均表现不佳。图11展示了在更复杂场景下的测试结果，这些场景中激光雷达和/或摄像头会偶尔出现退化现象，退化方向由相应箭头指示。“香港大学文化中心”[见图11(a)]展示了FAST-LIVO2、R3LIVE和FAST-LIVO的建图结果。可以看出，R3LIVE和FAST-LIVO的点云地图存在畸变、纹理模糊，且位置漂移超过1米。而FAST-LIVO2成功返回起点，端到端误差小于0.01米，同时生成了纹理清晰、一致性良好的点云地图。“CBD大楼03”[见图11(b)]和“采矿隧道”[见图11(c)]仅展示了FAST-LIVO2的结果，因为R3LIVE和FAST-LIVO无法完成建图。在图11(b)中，蓝色箭头表示朝向纯黑屏幕的移动方向，意味着激光雷达与摄像头同时发生退化。在图11(c1)和(c2)中，红点代表该位置的激光雷达扫描结果，显示了因仅能观测到单一平面而导致的激光雷达退化区域。此外，“采矿隧道”整个序列中的光线极为昏暗，且频繁出现视觉与激光雷达退化现象。尽管面临这些挑战，FAST-LIVO2仍能在两个序列中返回起点，端到端误差均小于0.01米。
### D. 高精度建图
在本实验中，我们验证了本系统的高精度建图能力。为了探究不同算法间的建图精度差异并确保公平性，我们在纹理丰富且结构复杂的场景中，将本系统与FAST-LIO2、R3LIVE以及FAST-LIVO进行对比。我们以“SYSU 01”、“HKU Landmark”和“CBD Building 01”这三个序列作为示例。补充材料中的图S9[^53]展示了这些序列实时重建的彩色点云地图。我们可以清晰地看到，在所有系统中，FAST-LIVO2生成的点云地图保留了最精细的细节，其彩色点云地图的放大视图与实际RGB图像中的效果极为相似。在“SYSU 01”序列中，我们的算法在标牌上产生的白噪点更少，因为我们先利用恢复的曝光时间将图像颜色归一化到合理的曝光水平，再进行着色处理，从而避免了彩色点云地图出现过度曝光的情况。在“CBD”场景中，人体和摩托车的重建效果也颇为出色。
$^{10}$
---
Fig. 11. Mapping results generated in real time in complex LiDAR degenerated and visually challenging scenes. (a), (b), and (c) correspond to “HKU Cultural Center,” “CBD Building 03,” and “Mining Tunnel,” respectively. Different colored arrows indicate the directions of degeneration caused by different sensors (see more details on YouTube). $^{11}$
---
TABLE III PROCESSING TIME (MS) PER LIDAR AND IMAGE FRAME
| Dataset | R3LIVE | LVI-SAM | FAST-LIVO | FAST-LIVO2 (LiDAR / Image) | FAST-LIVO2 (ARM) |
| Hilti'22 | | | | | |
| Construction Ground | 105.03 | ✗ | 52.33 | 36.52 (20.44 / 15.05) | 96.12 |
| Construction Multilevel | 112.13 | ✗ | 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 | ✗ | 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 | ✗ | 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 | ✗ | 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 | ✗ | 53.24 | 41.78 (26.12 / 15.66) | 94.43 |
| Basement | 118.21 | ✗ | 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 | ✗ | 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 | ✗ | 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 | ✗ | ✗ | ✗ | 21.53 (11.50 / 10.03) | 69.98 |
| CBD Building 03 | ✗ | ✗ | ✗ | 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 | ✗ | 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 | ✗ | 35.62 | 21.43 (11.42 / 10.01) | 72.43 |
| HKU Main Building | 75.62 | ✗ | 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 | ✗ | 39.98 | 21.71 (10.55 / 11.16) | 68.99 |
| Banner Wall | 74.13 | ✗ | 30.22 | 19.32 (10.01 / 9.31) | 63.43 |
| Bright Screen Wall | 73.23 | ✗ | 28.43 | 18.22 (9.92 / 8.30) | 61.21 |
| Black Screen Wall | 71.42 | ✗ | 27.66 | 17.99 (9.10 / 8.89) | 60.91 |
| Office Building Wall | ✗ | ✗ | ✗ | 19.42 (10.11 / 9.31) | 62.33 |
| Narrow Corridor | ✗ | ✗ | ✗ | 18.44 (8.32 / 10.12) | 61.42 |
| Long Corridor | ✗ | ✗ | ✗ | 19.53 (10.43 / 9.10) | 62.55 |
| Mining Tunnel | ✗ | ✗ | ✗ | 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 | ✗ | 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 |
× 表示该系统中途完全失效。
“CBD大楼01”序列同样体现了我们重建非结构化物体细节的能力。在所有序列中，估算的最终位置均能返回起点，端到端误差小于0.01米。我们还在私有数据集的其余序列中测试了FAST-LIVO2，建图结果展示在补充材料的图S10–S13中 [^53]。
### E. 运行时间分析
在本节中，我们评估了所提系统在桌面电脑上的平均计算时间，该电脑配备英特尔i7-10700K CPU和32GB内存，测试对象为每次激光雷达扫描和图像帧。我们的评估涵盖了Hilt $^{22}$、Hilt $^{23}$、NTU-VIRAL等公开数据集以及我们的私有数据集。如所示：
Fig. 12. Fully onboard UAV navigation algorithm flowchart.
表三显示，在所有序列中我们的系统处理时间最短。在英特尔i7处理器上，平均计算时间仅为30.03毫秒（每次激光雷达扫描17.13毫秒，每次图像帧12.90毫秒），能够满足10赫兹的实时操作要求。此外，我们的系统甚至能在ARM处理器上实现实时运行，每帧平均处理时间仅为78.44毫秒。LVI-SAM中的激光雷达与视觉特征提取模块（即LIO和VIO）耗时较长。除了LIO和VIO消耗的时间外，LVI-SAM还会在因子图中整合惯性测量单元预积分约束、视觉里程计约束以及激光雷达里程计约束，这进一步增加了总体处理时间。对于R3LIVE而言，尽管它同样采用直接法，但其像素级图像对齐需要大量视觉地图点。相比之下，我们的方法使用带有参考块的稀疏点，从而实现高效对齐。此外，R3LIVE会维护一个需进行贝叶斯更新的彩色地图，随着地图分辨率提高，计算负荷也会显著增大。至于FAST-LIO2，由于无需处理额外的图像数据，其每帧平均处理时间（详见补充材料中的表S3 [^53]，受篇幅限制未完全展示）比FAST-LIVO2少约10.35毫秒。
FAST-LIVO2相比其前身FAST-LIVO也有明显改进。主要提升源于我们在稀疏图像对齐中应用了逆合成公式。利用基于激光雷达点的平面先验进行仿射变形，进一步提高了我们方法的收敛效率。因此，FAST-LIVO2将每个金字塔层的迭代次数从10次减少至3次，同时仍能保证更高的精度。
### X. 应用场景
为了展示FAST-LIVO2在现实应用中的卓越性能与多功能性，我们开发了多种解决方案，包括全机载自主无人机导航、航空测绘、纹理网格生成，以及用于三维场景表示的三维高斯泼溅（3DGS）重建。
### A. 全机载自主无人机导航
鉴于FAST-LIO2的高精度、强大的定位性能以及实时能力，我们开展了闭环自主无人机飞行测试。
---
Fig. 13. (a) and (b) are the enlarged point maps of the “Woods” and “Narrow Opening” experiments, respectively. The red points in (a1), (a3), and (b4) represent the current scan. (a2) and (a4) represent the first-person view at the corresponding locations. (b1), (b2), and (b3) depict the third-person view (see more details on YouTube). $^{12}$
Fig. 14. Processing time of each module and in total in UAV autonomous navigation experiments across “Basement,” “Woods,” “Narrow Opening,” and “SYSU Campus.” The MPC executes at 100 Hz while planning and FAST-LIVO2 execute at 10 Hz, so its computation time is counted for ten times.
1) 系统配置：硬件与软件设置如图12所示。硬件方面，我们使用NUC（英特尔i7-1360P CPU和32 GB内存）作为机载计算机。软件方面，定位组件由FAST-LIVO2驱动，能以10赫兹的频率提供位置反馈。定位结果会被传输给飞行控制器，从而实现位置、速度和姿态200赫兹的反馈。除定位功能外，FAST-LIVO2还会向规划模块——Bubble规划器 [^55] 提供密集的配准点云，由其规划出平滑轨迹，再由流形上的模型预测控制（MPC） [^56] 进行跟踪。MPC会计算出期望的角速率与推力，这些参数由飞行控制器上运行的相应低层级角速率控制器予以跟踪。重要的是，MPC、规划器和FAST-LIVO2均在机载计算机上实时运行。
Fig. 15. (a) and (b) are the mesh and texture mapping of "CBD Building 01," respectively. (c) is the texture mapping of "Retail Street," with (c1) and (c2) showing local details.
2) 无人机自主导航：我们开展了四项完全机载自主无人机导航实验，分别为“地下室”、“树林”、“狭窄通道”和“中山大学校园”（详见补充材料中的表S2 [^53]）。“地下室”和“树林”实验为完全自主飞行，集成了规划、MPC以及FAST-LIVO2模块；而“狭窄通道”和“中山大学校园”则为手动飞行，仅使用MPC和FAST-LIVO2（不含规划模块）。可以看出，“地下室”和“树林”实验展示了无人机成功的自主导航与避障能力。在“狭窄通道”实验中，无人机被指令贴近墙壁飞行，导致激光雷达测量点较少。不过，光线投射模块能够调用更多的视觉地图点，为定位提供了丰富的约束条件，从而实现稳定的定位。此外，“地下室”和“狭窄通道”实验中存在激光雷达退化现象，只能观测到一面墙[参见图1(e1)和(e4)、图13(b1)–(b4)]，且光照变化显著[参见图1(e5)–(e6)]。尽管面临这些挑战，我们的无人机系统仍表现出色。“树林”实验中无人机以高达3米/秒的速度飞行，这对整个无人机系统的响应速度提出了极高要求[参见图13(a1)–(a4)]。“中山大学校园”属于非退化场景，主要用于展示机载高精度测绘能力（详见补充材料中的图S14 [^53]）。最后值得一提的是，在这四次无人机飞行中均出现了严重的光照变化，而FAST-LIVO2能够估算出与真实值极为接近的曝光时间（详见补充材料中的图S15 [^53]）。
关于机载计算时间，由于需要在机载计算机上以100 Hz的频率运行MPC，并以10 Hz的频率运行规划模块，这会占用计算资源和内存，从而限制了FAST-LIVO2可使用的计算资源。尽管控制与规划模块同时执行，但如图14所示，FAST-LIVO2每次激光雷达扫描和图像帧的平均机载处理时间约为53.47毫秒，仍远低于100毫秒的帧周期。规划模块和MPC的平均处理时间分别为8.43毫秒和18.5毫秒。总平均处理时间80.4毫秒完全满足机载操作的实时性要求。
---
Fig. 16. Comparison of ground-truth image, COLMAP+3DGS, and FAST-LIVO2+3DGS in terms of render details, computational time (time for generating point clouds and estimating poses + training time), and PSNR for a random frame in "CBD Building 01" (see more details on YouTube. $^{13}$) (a) Ground-truth image. (b) COLMAP + 3DGS. (c) FAST-LIVO2 + 3DGS.
### B. 空中测绘
航空测绘在测绘应用中是一项至关重要的任务。为了评估FAST-LIVO2在该应用中的适用性，我们利用公开数据集MARS-LVIG [^52]开展了航空测绘实验，其硬件配置详见第VIII-A节。我们选取了“HKairport01”和“HKisland01”两个序列进行评估，其实时测绘结果如图1(a)–(c)所示：其中图1(a)和(c)对应“HKisland01”，图1(b)则展示“HKairport01”的测绘结果。实验结果证明了FAST-LIVO2在非结构化环境（如森林和岛屿）中的有效性。该系统能够精准捕捉诸多精细结构和鲜明的色彩效果，包括建筑物、道路标线、路缘石、树冠以及岩石等，且这些细节均清晰可见。FAST-LIVO2在这两组序列上的APE（均方根误差）分别为0.64米和0.27米，而R3LIVE对应的数值则为2.76米和0.52米。在台式电脑上运行时的平均处理时间（详见第IX-A节）分别约为25.2毫秒和21.8毫秒，相比之下，R3LIVE的处理时间分别为110.5毫秒和100.2毫秒。
### C. 支持三维场景应用：网格生成、纹理映射与高斯泼溅
借助FAST-LIVO2提供的高精度传感器定位结果以及密集的3D彩色点云，我们开发了用于渲染流程的软件应用，包括网格生成与纹理映射，以及新兴的类似NeRF的渲染流程，如3DGS。在网格生成方面，我们在“CBD Building 01”中采用了基于截断有符号距离函数的VDBFusion [^57]，如图15(a)所示。柱子的锐利边缘和屋顶的独特结构清晰可见，充分体现了该网格的高质量。这种精细度得益于FAST-LIVO2点云的高密度以及结构重建的卓越精度。网格构建完成后，我们利用OpenMVS [^58]，结合在“CBD Building 01”和“Retail Street”中估算出的相机位姿来进行纹理映射，如图15(b)–(c)所示。在图15(c1)–(c2)中，应用于三角形面的纹理图像无缝且对齐精准，从而实现了清晰度极高、精度出色的纹理映射效果。这归功于FAST-LIVO2所实现的像素级图像对齐技术。
FAST-LIVO2生成的密集彩色点云也可直接作为3DGS的输入。我们在“CBD Building 01”序列中进行了测试，共使用了1180张图像中的300帧。测试结果如图16所示。与COLMAP [^59]相比，我们的方法将获取密集点云及位姿所需的时间从9小时大幅缩短至21秒。不过，训练时间则从10分59秒增加到了15分30秒。时间增加的原因是点云密度更高（下采样至5厘米），从而引入了更多需要优化的参数。尽管如此，由于点云密度和精度的提升，其峰值信噪比（PSNR）仍略高于使用COLMAP输入所得的PSNR值。
### 十一、结论与未来工作
本文提出了FAST-LIVO2，这是一种能够实时进行快速、精准且鲁棒的状态估计并同步构建地图的直接LIVO框架。FAST-LIVO2在具备高定位精度的同时，对严重的激光雷达或视觉退化现象也具有很强的鲁棒性。
速度上的提升得益于在高效的ESIKF框架内利用原始的激光雷达、惯性及相机数据，并采用顺序更新方式。在图像更新环节，进一步采用了逆合成公式以及基于稀疏块的图像对齐技术来提升效率。精度的提升则源于对激光雷达点云所提供平面先验的利用（甚至优化），从而增强图像对齐的精度。此外，系统采用单一的统一体素地图来同时管理地图点与观测到的高分辨率图像数据。这种支持几何构建与更新、视觉地图点生成与更新以及参考块更新的体素地图结构已经得到开发并验证。鲁棒性的提升源于曝光时间的实时估算，该技术能有效应对环境光照变化；同时，按需进行的体素光线投射可解决激光雷达的近距离盲区问题。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 Trans. Robot., vol. 33, no. 5, pp. 1255–1262, Oct. 2017.
[^2]: J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 40, no. 3, pp. 611–625, Mar. 2018.
[^3]: J. Engel, T. Schöps, and D. Cremers, "LSD-SLAM: Large-scale direct monocular SLAM," in Proc. Eur. Conf. Comput. Vis., 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 Trans. Robot., vol. 33, no. 2, pp. 249–265, Apr. 2017.
[^5]: J. Zhang and S. Singh, "LOAM: Lidar odometry and mapping in real-time," in Proc. Conf. Robot.: Sci. Syst., 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 Proc. IEEE Int. Conf. Robot. Automat., 2020, pp. 3126–3131.
[^7]: T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 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 Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2022, pp. 4003–4009.
[^9]: T. Qin, P. Li, and S. Shen, "VINS-Mono: A robust and versatile monocular visual-inertial state estimator," IEEE Trans. Robot., vol. 34, no. 4, pp. 1004–1020, Aug. 2018.
[^10]: R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “ORB-SLAM: A versatile and accurate monocular SLAM system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Oct. 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 Proc. IEEE Int. Conf. Robot. Automat., 2014, pp. 15–22.
[^13]: W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, "FAST-LIO2: Fast direct lidar-inertial odometry," IEEE Trans. Robot., vol. 38, no. 4, pp. 2053–2073, Aug. 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 Robot. Automat. Lett., vol. 7, no. 3, pp. 8518–8525, Jul. 2022.
[^15]: M. Meilland, A. I. Comport, and P. Rives, “Real-time dense visual tracking under large lighting variations,” in Proc. Brit. Mach. Vis. Conf. Brit. Mach. Vis. Assoc., 2011, pp. 45–1.
[^16]: T. Tykkälä, C. Audras, and A. I. Comport, “Direct iterative closest point for real-time visual odometry,” in Proc. IEEE Int. Conf. Comput. Vis. Workshops, 2011, pp. 2050–2056.
[^17]: C. Kerl, J. Sturm, and D. Cremers, “Robust odometry estimation for RGB-D cameras,” in Proc. IEEE Int. Conf. Robot. Automat., 2013, pp. 3748–3754.
[^18]: J. Engel, J. Sturm, and D. Cremers, “Semi-dense visual odometry for a monocular camera,” in Proc. IEEE Int. Conf. Comput. Vis., 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 Proc. IEEE Int. Conf. Robot. Automat., 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 Trans. Multimedia, vol. 25, pp. 3905–3920, 2023.
[^21]: J. Zhang and S. Singh, “Laser-visual-inertial odometry and mapping with high robustness and low drift,” J. Field Robot., 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 Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2019, pp. 370–377.
[^23]: J. Zhang, M. Kaess, and S. Singh, “A real-time method for depth enhanced visual odometry,” Auton. Robots, vol. 41, pp. 31–43, 2017.
[^24]: J. Graeter, A. Wilczynski, and M. Lauer, “LIMO: Lidar-monocular visual odometry,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 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 Proc. IEEE Int. Conf. Robot. Automat., 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 Proc. IEEE Int. Conf. Robot. Automat., 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 Trans. Robot., vol. 37, no. 6, pp. 1874–1890, Dec. 2021.
[^28]: Y.-S. Shin, Y. S. Park, and A. Kim, “DVL-SLAM: Sparse depth enhanced direct visual-lidar SLAM,” Auton. 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 Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2019, pp. 5848–5854.
[^30]: K. Sun et al., “Robust stereo visual inertial odometry for fast autonomous flight,” IEEE Robot. Automat. Lett., vol. 3, no. 2, pp. 965–972, Apr. 2018.
[^31]: X. Zuo et al., “LIC-Fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2020, pp. 5112–5119.
[^32]: D. Wisth, M. Camurri, S. Das, and M. Fallon, “Unified multimodal landmark tracking for tightly coupled lidar-visual-inertial odometry,” IEEE Robot. Automat. Lett., vol. 6, no. 2, pp. 1004–1011, Apr. 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 Robot. Automat. Lett., vol. 6, no. 4, pp. 7469–7476, Oct. 2021.
[^34]: B. M. Bell and F. W. Cathey, “The iterated Kalman filter update as a Gauss-Newton method,” IEEE Trans. Autom. Control, vol. 38, no. 2, pp. 294–297, Feb. 1993.
[^35]: T. Shan, B. Englot, C. Ratti, and D. Rus, "LVI-SAM: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping," in Proc. IEEE Int. Conf. Robot. Automat., 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 Proc. Int. Conf. Robot. Automat., 2022, pp. 10672–10678.
[^37]: J. Lin and F. Zhang, "R 3 Live++: A robust, real-time, radiance reconstruction package with a tightly-coupled lidar-inertial-visual state estimator," IEEE Trans. Pattern Anal. Mach. Intell., 2024.
[^38]: J. Engel, V. Usenko, and D. Cremers, “A photometrically calibrated benchmark for monocular visual odometry,” 2016, arXiv:1607.02555.
[^39]: W. Wang, J. Liu, C. Wang, B. Luo, and C. Zhang, "DV-Loam: Direct visual lidar odometry and mapping," Remote Sens., vol. 13, no. 16, 2021, Art. no. 3340.
[^40]: Z. Yuan, Q. Wang, K. Cheng, T. Hao, and X. Yang, “SDV-Loam: Semi-direct visual-lidar odometry and mapping,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 45, no. 9, pp. 11203–11220, Sep. 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 Robot. Automat. Lett., vol. 9, no. 4, pp. 3783–3790, Apr. 2024.
[^42]: W. Xu and F. Zhang, "FAST-LIO: A fast, robust lidar-inertial odometry package by tightly-coupled iterated Kalman filter," IEEE Robot. Automat. Lett., vol. 6, no. 2, pp. 3317–3324, Apr. 2021.
[^43]: D. He, W. Xu, and F. Zhang, “Symbolic representation and toolkit development of iterated error-state extended Kalman filters on manifolds,” IEEE Trans. Ind. Electron., vol. 70, no. 12, pp. 12533–12544, Dec. 2023.
[^44]: D. Willner, C.-B. Chang, and K.-P. Dunn, “Kalman filter algorithms for a multi-sensor system,” in Proc. IEEE Conf. Decis. Control Including 15th Symp. Adaptive Processes, 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,” Inf. Fusion, vol. 99, 2023, Art. no. 101885.
## [^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,” 2023, arXiv:2302.14819
---
[^47]: R. M. Stereopsis, “Accurate, dense, and robust multiview stereopsis,” IEEE Trans. Pattern Anal. Mach. Intell., vol. 32, no. 8, pp. 1362–1376, Aug. 2010.
[^48]: S. Baker and I. Matthews, "Lucas-Kanade 20 years on: A unifying framework," Int. J. Comput. Vis., 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,” Int. J. Robot. Res., 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 Robot. Automat. Lett., vol. 7, no. 3, pp. 7518–7525, Jul. 2022.
[^51]: L. Zhang et al., “Hilti-Oxford dataset: A millimeter-accurate benchmark for simultaneous localization and mapping,” IEEE Robot. Automat. Lett., vol. 8, no. 1, pp. 408–415, Jan. 2023.
[^52]: H. Li et al., “MARS-LVIG dataset: A multi-sensor aerial robots SLAM dataset for lidar-visual-inertial-GNSS fusion,” Int. J. Robot. Res., vol. 43, no. 8, 2024, Art. no. 02783649241227968.
[^53]: “Supplementary material: Fast-livo2: Fast, direct lidar-inertial-visual odometry,” Aug. 2024. [Online]. Available:
[^54]: C. Klug, C. Arth, D. Schmalstieg, and T. Gloor, "Measurement uncertainty analysis of a robotic total station simulation," in Proc. IECON 44th Annu. Conf. IEEE Ind. Electron. Soc., 2018, pp. 2576–2582.
[^55]: Y. Ren et al., “Bubble planner: Planning high-speed smooth quadrotor trajectories using receding corridors,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., 2022, pp. 6332–6339.
[^56]: G. Lu, W. Xu, and F. Zhang, “On-manifold model predictive control for trajectory tracking on robotic systems,” IEEE Trans. Ind. Electron., vol. 70, no. 9, pp. 9192–9202, Sep. 2023.
[^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, Art. no. 1296. [Online]. Available:
[^58]: D. Cernea, “OpenMVS: Multi-view stereo reconstruction library 2020,” 2020. [Online]. Available:
[^59]: J. L. Schönberger and J.-M. Frahm, “Structure-from-motion revisited,” in Proc. IEEE Conf. Comput. Vis. Pattern Recognit., 2016, pp. 4104–4113.
邹祖豪于2016年在英国谢菲尔德大学获得自动控制系统工程专业的工学学士学位，2017年于伦敦大学学院获得计算机视觉、图形与成像专业的理学硕士学位。他目前正于中国香港大学攻读机械工程专业的博士学位。
他的研究兴趣包括机器人技术、传感器融合、定位与建图以及回环检测。
华彤于2021年在中国上海交通大学获得电子工程专业的学士学位。自2021年起，他一直在上海交通大学导航与位置服务上海市重点实验室攻读信息与通信工程专业的硕士学位。
他的研究兴趣包括多传感器融合以及视觉惯性里程计。
袁崇健于2016年在中国浙江大学控制科学与工程学院获得自动化专业的工学学士学位。他目前正于中国香港大学机械工程系攻读机器人技术专业的博士学位。
他的研究兴趣包括激光雷达SLAM以及传感器融合。
郑春然（IEEE学生会员）于2020年在中国西安交通大学获得自动化专业的工学学士学位。他目前正于中国香港大学机械工程系攻读机器人与SLAM专业的博士学位。
他的研究兴趣包括多传感器校准、传感器融合、激光雷达-惯性-视觉SLAM以及3D高斯泼溅技术。
何冬娇（IEEE会员）于2016年在中国东南大学获得机械工程专业的学士学位，2019年在上海交通大学获得机械工程专业的硕士学位，并于2024年7月在香港大学张福教授指导下获得机器人技术专业的博士学位。
她博士阶段的研究兴趣包括用于自主导航的高级多传感器算法、SLAM以及无人机相关领域。她致力于推动自主系统的研究，涵盖多源信息融合、自主感知与决策制定等方面。
徐伟于2022年在中国香港大学获得机械工程专业的博士学位。
他目前担任中国香港Manifold Tech Limited公司的首席技术官。他的研究与专长包括滤波算法、多传感器融合、同步定位与密集建图、无人机系统以及运动规划。
周炳阳（IEEE会员）于2023年在中国哈尔滨工业大学深圳校区获得自动化专业的工学学士学位。他目前正于中国香港大学机械工程系MaRS实验室攻读机械工程专业的哲学硕士学位。
他的研究重点是同时定位与地图构建。
---
郑刘（IEEE会员）于2019年在中国哈尔滨获得哈尔滨工业大学自动化专业工学学士学位，并于2024年在中国香港获得香港大学机械工程博士学位。
他的研究兴趣包括基于视觉或激光雷达的定位与地图构建，以及传感器融合与校准。
王蓉于2013年在中国北京获得北京航空航天大学自动化专业理学学士学位，并于2018年6月在中国北京获得中国科学院自动化研究所自动化专业博士学位。
她的博士研究工作聚焦于SLAM与增强现实技术。自2018年7月起，她任职于中国电子科技集团公司信息科学研究院，地点为中国北京。她的研究兴趣包括语义SLAM与无人机。
林家荣（IEEE会员）于2015年在中国成都获得电子科技大学光电信息科学与技术专业理学学士学位。他目前正于中国香港香港大学机械工程系攻读机器人学博士学位。
他的研究兴趣包括激光探测与测距地图构建以及传感器融合。
孟凡乐于2010年在中国武汉获得华中科技大学生物医学工程专业理学学士学位，并于2018年6月在中国北京获得清华大学生物医学工程博士学位。
她的博士研究工作聚焦于医疗机器人与手术导航。自2018年7月起，她任职于中国电子科技集团公司信息科学研究院，地点为中国北京。她的研究兴趣包括SLAM与地面无人车辆。
朱方成（IEEE学生会员）于2021年在中国深圳获得哈尔滨工业大学机械工程及自动化学院自动化专业工学学士学位。他目前正于中国香港香港大学机械工程系攻读机器人学博士学位。
他的研究兴趣包括基于激光雷达的同时定位与地图构建、传感器校准以及空中集群系统。
张富于2011年在中国合肥获得中国科学技术大学自动化专业工学学士学位，并于2015年在美国加州伯克利获得加州大学伯克利分校控制专业博士学位。
他的博士研究工作聚焦于微速率积分陀螺仪的自校准与控制。2016年，他在香港科技大学机器人研究所担任无人机设计与控制方向的研究助理教授。自2018年8月起，他担任香港大学机械工程系的助理教授。他的研究兴趣包括机器人与控制领域，重点涉及无人机设计、导航、控制以及基于激光雷达的同时定位与地图构建。
任云帆（IEEE学生会员）于2021年在中国哈尔滨获得哈尔滨工业大学自动化专业工学学士学位。他目前正于中国香港香港大学机械工程系攻读博士学位。
他是香港MaRS实验室的成员。他的研究兴趣包括自主导航、无人机规划、最优控制以及群体系统自主性。
任先生曾在ICRA 2023上被评为杰出导航论文入围者，并在IROS 2023上同时入选最佳综合论文入围者和最佳学生论文入围者。