最新开源！在测绘无人机，机器人，手持设备上都能鲁棒建图的FAST-LIVO2

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

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

FAST-LIVO2 是基于之前工作中首次提出的 FAST-LIVO 开发的，与 FAST-LIVO 相比，新版本的贡献如下：

• 提出了一个高效的ESIKF框架，通过顺序更新解决了激光雷达和视觉测量之间的维度不匹配问题，提高了使用异步更新的FAST-LIVO的鲁棒性。

• 利用和优化了来自激光雷达点的平面先验来提高精度。相比之下FAST-LIVO 假设一个块中的所有像素共享相同的深度，这一不切实际的假设大大降低了图像对齐中仿射变换的准确性。

• 提出了一种参考图像块更新策略，通过选择具有大视差和足够纹理细节的高质量内点参考图像块，来提高图像对齐的准确性，FAST-LIVO 基于与当前视图的接近程度选择参考图像块，往往导致选择质量低的参考图像块，进而降低对齐的准确性。

• 进行在线曝光时间估计，以应对环境光照变化，FAST-LIVO 未解决此问题，导致在显著光照变化下图像对齐收敛性较差。

• 提出了按需体素射线投射，以增强系统在由于激光雷达近距离盲区导致的激光雷达点测量缺失情况下的鲁棒性，而FAST-LIVO并未考虑这一问题。

上述每项贡献都通过全面的消融研究进行了评估，以验证其有效性。将提出的系统实现为实用的开源软件，并经过精心优化，以便在 Intel 和 ARM 处理器上实现实时运行。该系统功能强大，支持多线旋转激光雷达、新兴的具有非传统扫描模式的固态激光雷达，以及针孔相机和各种鱼眼相机。此外在25个公共数据集（如Hilti和NTU-VIRAL数据集）序列以及各种具有代表性的私有数据集上进行了广泛实验，从而与其他最先进的SLAM系统（如R3LIVE、LVI-SAM、FAST-LIO2等）进行了比较。定性和定量结果表明，我们提出的系统在降低计算成本的同时，在准确性和鲁棒性方面显著优于其他系统。为了进一步强调系统在现实世界中的适用性和多功能性，我们部署了三个独特的应用。首先完全机载自主无人机导航，展示了系统的实时能力，标志着首次在真实世界自主无人机飞行中采用激光雷达-惯性-视觉系统。其次空中测绘展示了系统在无结构环境中实际应用中的像素级精度。最后高质量的网格生成、纹理化和NeRF模型生成强调了系统在渲染任务中的适用性，在GitHub上公开了我们的代码和数据集。

概述

异步采样的激光雷达点首先通过扫描重组在摄像头的采样时间被重新组合成扫描点云数据。然后通过具有顺序状态更新的ESIKF紧密结合激光雷达、图像和惯性测量，其中系统状态依次通过激光雷达测量和图像测量进行更新，二者均利用基于单一统一体素地图的直接方法。为了在ESIKF更新中构建激光雷达测量模型，计算帧到地图的点到平面残差。为了建立视觉测量模型，从地图中提取当前视场内的视觉地图点，使用可见体素查询和按需射线投射；在提取后识别并丢弃视觉地图中的异常点（例如，被遮挡或表现出深度不连续性的点）；然后计算帧到地图的图像光度误差以进行视觉更新。视觉和激光雷达更新所使用的局部地图是一个体素地图结构：激光雷达点构建并更新地图的几何结构，而视觉图像将图像块附加到选定的地图点（即视觉地图点）上，并动态更新参考图像块，更新后的参考图像块的法向量会在单独的线程中进一步优化。

图2，FAST-LIVO2系统结构图

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

扫描重组与传播

首先，通过扫描重组将高频率采样的激光雷达点分段为与摄像头采样时刻同步的扫描数据。这确保了激光雷达和摄像头数据的同步性，使得在同一时间点上更新系统状态。在 ESIKF（误差状态迭代卡尔曼滤波器）框架中，状态和协方差从接收到上一帧激光雷达和图像数据的时间传播到接收到当前帧数据的时间。这一过程通过前向传播预测状态，同时通过反向传播补偿运动畸变，确保激光雷达扫描中的点在扫描结束时刻被正确“测量”。

顺序更新策略

ESIKF 采用了一种顺序更新策略，以应对激光雷达和图像测量之间的维度不匹配问题，并为各个模块提供更多的灵活性。该策略首先将 IMU 传播的先验分布与激光雷达测量进行融合，生成初步的后验分布。接着，再将摄像头测量与这一后验分布进行融合，获得最终的状态估计。这种顺序更新确保了不同感知模式之间的有效融合，同时提高了系统的鲁棒性和灵活性。

实现与优化

顺序更新策略的实现包含两个主要步骤。第一步是利用激光雷达测量来迭代更新误差状态，直到收敛。这一收敛的状态和协方差随后用于更新地图的几何结构。第二步是在图像金字塔的每一层上进行视觉更新，直至收敛。最终的状态和协方差用于处理新的 IMU 测量，并进一步优化地图的视觉结构。该方法通过有效的顺序更新提高了系统的精度和计算效率，并在不同的传感器数据之间实现了无缝融合。

图 3：说明了应用于输入数据的扫描重组、前向传播和反向传播

局部地图构建

地图结构

地图采用了一种自适应的体素结构，该结构由哈希表和每个哈希条目中的八叉树组织管理。哈希表管理根体素，每个根体素的固定尺寸为0.5×0.5×0.5米。每个根体素包含一个八叉树结构，用于进一步组织不同大小的叶体素。叶体素表示局部平面，并存储平面特征（如平面中心、法向量和不确定性），以及位于该平面上的一组激光雷达原始点。某些点附加了三层图像补丁（8×8像素大小），称为视觉地图点。收敛的视觉地图点仅附加参考补丁，而未收敛的则附加参考补丁和其他可见补丁。为了防止地图尺寸无限增长，我们仅在激光雷达当前位置周围的一个大局部区域内保留局部地图。最初，地图是以激光雷达起始位置为中心的立方体。当激光雷达移动到新位置并且检测区域触及地图边界时，我们将地图从边界移动一段距离，并重置移出局部地图的区域，以存储新进入的区域。这种环形缓冲策略确保地图在固定大小的内存中维持。地图移动检查在每次 ESIKF 更新步骤后执行。

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

几何结构的构建与更新

地图的几何结构通过激光雷达点云测量来构建和更新。在 ESIKF 激光雷达更新后，我们将所有激光雷达扫描点配准到全局坐标系中。对于每个已配准的激光雷达点，我们确定其在哈希表中的根体素位置。如果体素不存在，则使用新点初始化该体素并将其索引到哈希表中；如果体素已存在，则将新点附加到现有体素中。

对于新创建的体素，我们通过奇异值分解确定所有包含的点是否位于平面上。如果是，则计算平面中心点、法向量及其协方差矩阵，并将这些参数用于描述平面的不确定性。如果点不在平面上，体素将被继续细分，直到满足条件或达到最大层级。对于已有的体素，如果新附加的点仍与现有点形成平面，则更新平面参数和协方差。一旦平面参数收敛，平面将被视为成熟，新的点将不再附加到该平面。

视觉地图点的生成与更新

为了生成和更新视觉地图点，我们选择地图中当前帧可见的、在当前图像中表现出显著灰度梯度的候选激光雷达点。这些候选点将在视觉更新后投影到当前图像中，并保留每个体素中具有最小深度的点。然后，我们将当前图像分为每个单元30×30像素的网格。如果一个网格单元没有视觉地图点投影到此处，我们使用灰度梯度最高的候选点生成新的视觉地图点，并将其与当前图像补丁、估计的当前状态（即帧姿态和曝光时间）以及激光雷达点计算的平面法向量关联。

参考补丁的更新

由于视觉地图点可能有多个补丁，因此我们需要选择一个参考补丁用于图像对齐。我们基于光度相似性和视角对每个补丁进行评分，并选择得分最高的补丁作为参考补丁。评分机制倾向于选择那些在外观上与大多数其他补丁相似且视角与平面垂直的补丁。

图5展示了两种主要的变换过程：仿射变换：图5（a）展示了如何通过仿射变换将参考补丁中的像素建图到目标补丁中的像素。法线转换：图5（b）展示了法线向量的转换过程。首先，将法线向量从球面投影到平面上，然后再投影到二维平面上。这个过程将球面上的小扰动转换为二维平面上的小扰动。

法向量的细化

每个视觉地图点假定位于一个局部平面上。为提高图像对齐中的仿射变换精度，平面法向量可以通过最小化光度误差从附加到视觉地图点的补丁中进一步细化。一旦平面法向量收敛，该视觉地图点的参考补丁和法向量将被固定，所有其他补丁将被删除。

激光雷达测量模型

点到平面的LiDAR测量模型

在进行LiDAR更新时，我们首先将去畸变的LiDAR点投影到全局坐标系，并确定这些点所在的体素。如果该体素中存在平面，我们会利用该平面来建立LiDAR点的测量方程。测量方程的基础是假设真实的LiDAR点应当位于体素中定义的平面上。实际的LiDAR点会受到测距噪声的影响，因此测量模型会考虑这些噪声因素。通过将点的实际测量值与平面的参数进行比较，可以得到一个测量误差值，该误差值用于优化LiDAR的姿态估计。

带有光束发散的LiDAR测量噪声

LiDAR点的测距不确定性包括激光飞行时间引起的测距误差以及方位方向的不确定性。此外，激光光束的发散角度也会影响测距不确定性。当激光光束与目标平面的法向量的角度增大时，测距的不确定性会显著增加。系统通过考虑光束发散的影响，可以提高姿态估计的精确度，尤其是当从地面或墙壁等固定物体上采集更多点时，这种改进尤为明显。如图6所示。

图6：(a) 和 (b) 分别展示了考虑激光光束发散角度θ的LiDAR点不确定性模型的3D视图和侧面截面视图。红色轮廓标出了激光光束扩散的区域。(c) 和 (d) 用不同颜色表示扫描中点的位置不确定性。与 (c) 相比，(d) 进一步考虑了由于光束发散角度引起的测距不确定性δd。这导致地面点的不确定性较高，因为激光光束的扩散区域较大。

视觉地图点和建立稀疏-直接视觉测量模型

视觉地图点的处理

1. 视觉地图点筛选：

• 将子地图中的视觉地图点投影到当前帧，使用LiDAR更新后的姿态，并在每个30 × 30像素的网格单元中保留最低深度的点。

• 通过将当前LiDAR扫描中的点投影到当前帧，生成深度图。比较视觉地图点在深度图中的深度与其9 × 9邻域中的点，确定其遮挡和深度变化。被遮挡和深度不连续的地图点会被丢弃。

• 对于视角过大的点（例如，视角大于80°），也会被移除。这些视觉地图点用于对齐当前图像。

稀疏-直接视觉测量模型

1. 构建视觉测量模型：

• 使用提取的视觉地图点来构建视觉测量模型。理论上，当将地图点转换到当前图像时，参考贴图和当前贴图之间的光度误差应为零。

• 采用逆复合方法来提高计算效率，其中姿态增量参数化用于优化计算。通过一次计算雅可比矩阵来避免每次迭代都重新计算。

2. 曝光时间估计：

• 固定初始逆曝光时间以避免方程退化，后续帧的逆曝光时间则相对于第一帧进行估计。

3. 视觉更新步骤：

• 在三个级别上使用视觉更新步骤，从最粗的级别开始，直到收敛后进入更细的级别。更新后的状态用于生成视觉地图点和更新参考贴图。

图7：按需体素射线投射的示意图

图 8：异常值剔除。（a）显示被遮挡和深度不连续的视觉地图点的示意图。（b）显示了在真实场景中异常值拒绝的效果。红点是被拒绝的视觉地图点，绿点是被接受的视觉地图点。

1. 公开数据集:

• NTU-VIRAL: 包含多样的空中操作场景，使用了16通道LiDAR和同步相机。主要挑战包括远距离噪声特征和复杂的飞行动态。

• Hilti'22 和 Hilti'23: 包含室内和室外环境的手持和机器人设备数据，面临长走廊、地下室和楼梯等挑战，数据包括LiDAR和相机的测量，真实数据由运动捕捉系统或全站仪提供。

• MARS-LVIG: 提供高空地面建图数据，涵盖丛林、山脉和岛屿等地形，使用高分辨率RGB图像和LiDAR，适用于验证高空航空测绘能力。

2. FAST-LIVO2 私有数据集:

• 平台: 配备工业相机、LiDAR和高性能计算机，所有传感器通过同步信号进行硬件同步。

• 序列描述: 包含20个具有挑战性的场景（如无纹理平面、极窄隧道等），总时长66.9分钟。大多数序列展示了视觉和/或LiDAR的衰退现象。数据集支持高同步数据采集和漂移评估。

实现与系统配置

在 C++ 和机器人操作系统 (ROS) 上实现了 FAST-LIVO2 系统。默认配置下，启用了曝光时间估计，而法线向量优化被关闭。LiDAR 扫描中的点以 1:3 的时间比例进行下采样。体素图的根体素大小设置为 0.5 米，内部八叉树的最大层数为 3。图像补丁的大小为 8 × 8 像素用于图像对齐，11 × 11 像素用于法线优化。在序列 ESIKF 设置中，所有实验中相机的光度噪声设置为常数值 100。LiDAR 的深度误差和方位角误差分别调整为 Livox Avia LiDAR 和 OS1-16 的 0.02 米和 0.05°，PandarXT-32 为 0.001 米和 0.001°，Robosense BPearl LiDAR 为 0.008 米和 0.01°。激光束发散角分别为 Livox Avia LiDAR 和 OS1-16 的 0.15°，PandarXT-32 和 Robosense BPearl LiDAR 的 0.001°。我们在所有数据集中使用相同的传感器设置和参数。计算平台为配备 Intel i7-10700K CPU 和 32 GB RAM 的桌面 PC。我们还在通常用于嵌入式系统的 ARM 处理器上测试了 FAST-LIVO2，该平台为 RB58，配备 Qualcomm Kryo585 CPU 和 8 GB RAM，我们将其称为“FAST-LIVO2 (ARM)”。

图9: 用于数据采集的硬件同步平台。(a) 手持平台，(b) 硬件同步方案

基准实验

对来自 NTU-VIRAL、Hilti’22 和 Hilti’23 的 25 个序列进行了定量评估。将我们的方法与多个先进的开源里程计系统进行比较，包括 R3LIVE（密集直接 LiDAR-惯性-视觉里程计系统）、FAST-LIO2（直接 LiDAR-惯性里程计系统）、SDV-LOAM（半直接 LiDAR-视觉里程计系统）、LVI-SAM（基于特征的 LiDAR-惯性-视觉 SLAM 系统）和我们之前的工作 FAST-LIVO。我们下载了这些系统的 GitHub 代码，并根据推荐设置进行配置。我们对 R3LIVE 进行了适配，以支持鱼眼相机模型和带有外部 IMU 的多线 LiDAR（默认配置仅支持内部 IMU）。由于 SDV-LOAM 的视觉模块是开源的，我们将其与 LeGO-LOAM 进行松耦合集成。与所有对比系统相比，FAST-LIVO2 在大多数序列中表现最佳，平均 RMSE 为 0.044 米，明显优于第二名 FAST-LIVO 的 0.137 米。FAST-LIVO2 在“外部建筑”和“暗大房间”序列中表现略逊一筹，主要由于这些序列的光照条件差，导致图像模糊。除这两个序列外，FAST-LIVO2 显著优于 FAST-LIO2 和 SDV-LOAM。特别是 SDV-LOAM 在 Hilti 数据集上的表现较差，因为其 IMU 测量与 LiDAR 观察的耦合较松，导致 LO 子系统的漂移。FAST-LIVO2 在所有序列中的精度明显高于其他紧耦合的 LiDAR-惯性-视觉系统。我们还进行了消融研究，分析了曝光时间估计模块、法线优化模块和参考补丁更新策略的效果。结果表明，默认的 FAST-LIVO2 系统在曝光时间估计和参考补丁更新方面具有最佳性能。

LiDAR 退化与视觉挑战环境

在此实验中，我们评估了系统在 LiDAR 退化和/或视觉挑战环境中的鲁棒性，并与 FAST-LIVO 和 R3LIVE 的定性建图结果进行比较。图 10 展示了 LiDAR 退化序列，其中 LiDAR 面对墙壁并沿墙壁移动，导致几何约束缺失。FAST-LIVO2 在长时间的退化中表现出色，能够提供高精度的彩色点云地图，而 FAST-LIVO 和 R3LIVE 的表现较差，特别是在“建筑墙面”和“矿道”序列中，FAST-LIVO2 依然能在极其狭窄的空间中返回起始点，误差低于 0.01 米。

图 10: 在 LiDAR 退化场景中实时生成的建图结果。自上而下的点云分别对应于“明亮屏幕墙”、“黑色屏幕墙”、“HIT 涂鸦墙”（第三行和第四行）、“横幅墙”和“HKU 讲座中心”，显示了 FAST-LIVO2、FAST-LIVO 和 R3LIVE 构建的彩色点云的比较

高精度建图

在高精度建图实验中，我们验证了系统的高精度建图能力。通过与 FAST-LIO2、R3LIVE 和 FAST-LIVO 的比较，我们在具有丰富纹理和结构化环境的序列中评估了建图精度。图 S9 展示了 FAST-LIVO2 生成的彩色点云地图，细节保留最为完整，重建效果最接近实际 RGB 图像。系统在所有测试序列中均能返回起始点，终点误差低于 0.01 米。

图 S9：在富有纹理和结构化场景中在线生成的实时建图结果。从左到右的点云分别对应“HKU 地标”、“SYSU 01”和“CBD 建筑 01”，展示了 FAST-LIVO2、FAST-LIVO、R3LIVE 和 FAST-LIO2 之间的彩色点云精度比较。

运行时间分析

我们评估了系统在每个 LiDAR 扫描和图像帧上的平均计算时间，测试平台为 Intel i7-10700K CPU 和 32 GB RAM 的桌面 PC。结果显示，我们的系统在所有序列中表现出最低的处理时间，每帧平均处理时间为 78.44 毫秒，即使在 ARM 处理器上也能实时操作。相较于其他系统，FAST-LIVO2 的运行效率有明显改进，主要得益于逆向组合公式的应用，减少了每层金字塔的迭代次数，从而提高了收敛效率。

完全车载自主无人机导航

利用 FAST-LIVO2 进行了一系列完全车载的自主无人机导航实验，以展示其在实际应用中的卓越性能。系统配置包括一个搭载 Intel i7-1360P CPU 和 32 GB RAM 的 NUC 作为车载计算机，以及 FAST-LIVO2 提供的定位组件，每秒 10 次位置反馈。定位结果被传输到飞控器，实现 200Hz 的位置、速度和姿态反馈。除了定位，FAST-LIVO2 还提供密集的点云数据，供 Bubble Planner 用于规划平滑轨迹，并由 On-Manifold Model Predictive Control (MPC) 进行跟踪。我们进行了四次完全自主导航实验，包括“地下室”、“树林”、“狭窄开口”和“SYSU 校园”。实验结果显示，无人机在“地下室”和“树林”中成功实现了自主导航和避障，而在“狭窄开口”中虽然面临 LiDAR 点测量少的问题，但仍能保持稳定定位。在高速飞行的“树林”实验中，系统的响应能力也表现优异。“SYSU 校园”展示了车载高精度建图能力。所有实验中均存在严重的光照变化，但 FAST-LIVO2 能有效估计曝光时间，处理时间也满足实时操作需求。

图 12：完全自主无人机导航算法流程图

空中建图

在空中建图应用中，我们使用公共数据集 MARS-LVIG 进行实验，以评估 FAST-LIVO2 的性能。实验包括两个序列：“HKairport01”和“HKisland01”，其实时建图结果展示了 FAST-LIVO2 在非结构化环境中的有效性。系统成功捕捉了建筑物、车道标记、路缘、树冠和岩石等细节，显示了其在森林和岛屿等复杂环境中的高效建图能力。FAST-LIVO2 的 APE（均方根误差）分别为 0.64 m 和 0.27 m，而 R3LIVE 为 2.76 m 和 0.52 m，显示出 FAST-LIVO2 更高的精度。桌面 PC 上的平均处理时间分别为 25.2 ms 和 21.8 ms，相比之下 R3LIVE 的处理时间更长。

图 13：(a) 和 (b) 分别是“Woods”和“Narrow Opening”实验的放大点云图。图 (a1)、(a3) 和 (b4) 中的红点表示当前扫描的位置。图 (a2) 和 (a4) 显示了相应位置的第一人称视角。图 (b1)、(b2) 和 (b3) 则展示了第三人称视角。

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

开发了多个基于 FAST-LIVO2 的软件应用程序，包括网格生成、纹理建图和 3D 高斯溅射（3DGS）。使用 VDBFusion 和 TSDF，我们在“CBD Building 01”中生成了高质量的网格，展示了柱子锐利的边缘和屋顶的细致结构。纹理建图使用 OpenMVS 完成，生成的纹理图像清晰准确，与三角形面的对齐效果很好。FAST-LIVO2 的点云还直接用于 3DGS，测试结果表明，相较于 COLMAP，我们的方法大幅度缩短了获取密集点云的时间，从 9 小时缩短到 21 秒，虽然训练时间略有增加，但点云的密度和精度提升使得 PSNR 稍高于 COLMAP 输入。

图 11：在复杂的 LiDAR 衰退和视觉挑战场景中实时生成的建图结果。(a)、(b) 和 (c) 分别对应“HKU 文化中心”、“CBD 建筑 03”和“矿井隧道”。不同颜色的箭头表示由不同传感器引起的衰退方向。

图 16：在“CBD Building 01”中的一个随机帧上，比较了真实图像、COLMAP+3DGS 和 FAST-LIVO2+3DGS 的渲染细节、计算时间（生成点云和估计姿态的时间 + 训练时间）以及峰值信噪比（PSNR）。

本文提出了 FAST-LIVO2，一种直接的 LIVO 框架，能够实现快速、准确和稳健的状态估计，同时动态重建地图，FAST-LIVO2 能够在严重的 LiDAR 和/或视觉退化条件下实现高定位精度。速度提升归因于在高效的 ESIKF 框架中使用原始 LiDAR、惯性和相机测量数据，并进行顺序更新。在图像更新中，采用逆组合形式和基于稀疏补丁的图像对齐方法进一步提高了效率。准确性的提升归因于利用（甚至细化）来自 LiDAR 点的平面先验，以增强图像对齐的精度。此外使用单一统一的体素地图同时管理地图点和观察到的高分辨率图像测量数据。体素地图结构支持几何构建和更新、视觉地图点生成和更新以及参考补丁更新，并经过开发和验证。稳健性的提升归因于实时曝光时间估计，这有效地处理了环境照明变化，以及按需体素射线投射以应对 LiDAR 的近距离盲区，FAST-LIVO2 的效率和准确性在广泛的公共数据集上进行了评估，而各系统模块的稳健性和有效性则在私人数据集上进行了评估。同时还展示了 FAST-LIVO2 在实际机器人应用中的应用，如无人机导航、3D 建图和模型渲染。作为一种里程计系统，FAST-LIVO2 可能在长距离上出现漂移。未来可以将回环闭合和滑动窗口优化集成到 FAST-LIVO2 中，以减轻这种长期漂移。此外，准确且密集的彩色点云可以用于提取语义信息，用于物体级语义建图。