FAST-LIVO2 Fast, Direct LiDAR-Inertial-Visual Odometry论文阅读

论文下载

论文下载地址

论文翻译

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

摘要

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

脚注1: https://github.com/hku-mars/FAST-LIVO2

关键词–同步定位与建图（SLAM），传感器融合，3D重建，空中导航。

I 引言

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

目前，已有多个SLAM框架成功实现了单传感器测量，主要是相机[1, 2, 3, 4]或LiDAR[5, 6, 7]。尽管视觉和LiDAR SLAM在各自的领域中表现出色，但每种方法都有其固有的局限性，限制了它们在不同场景中的表现。

视觉SLAM利用成本低廉的CMOS传感器和镜头，能够建立准确的数据关联，从而实现一定程度的定位精度。丰富的颜色信息进一步增强了语义感知。通过利用这种增强的场景理解，深度学习方法被用于鲁棒的特征提取和动态物体过滤。然而，视觉SLAM缺乏直接的深度测量，需要通过三角测量或深度过滤等操作同时优化地图点，这引入了显著的计算开销，通常限制了地图的精度和密度。视觉SLAM还面临许多其他限制，如不同尺度下的测量噪声变化、对光照变化的敏感性以及无纹理环境对数据关联的影响。

LiDAR SLAM利用LiDAR传感器直接获取精确的深度测量，在定位和建图任务中提供了比视觉SLAM更高的精度和效率。尽管有这些优势，LiDAR SLAM也存在一些显著的缺点。一方面，它重建的点云地图虽然详细，但缺乏颜色信息，从而降低了信息量。另一方面，LiDAR SLAM在几何约束不足的环境中（如狭窄的隧道、单一且延伸的墙壁等）表现往往不佳。

随着智能机器人在现实世界中操作的需求增长，尤其是在缺乏结构或纹理的环境中，现有的依赖单一传感器的系统无法提供所需的准确和鲁棒的姿态估计。为了解决这个问题，融合常用传感器（如LiDAR、相机和IMU）的策略越来越受到关注。这种策略不仅结合了这些传感器的优势，提供了增强的姿态估计，还有助于构建准确、密集且彩色的点云地图，即使在单个传感器性能退化的环境中也能保持良好表现。

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

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

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

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

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

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

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

II 相关工作

直接方法

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

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

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

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

LiDAR-视觉（-惯性）SLAM

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

Zhang等人提出了一种在状态估计级别松散耦合的LiDAR-视觉-惯性SLAM系统[21]。在该系统中，VIO子系统仅为LIO子系统中的扫描注册提供初始姿态，而不是与扫描注册联合优化。VIL-SLAM [22]采用了类似的松散耦合方法，不利用LiDAR、相机和IMU测量的联合优化。

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

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

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

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

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

III 系统概述

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

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

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

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

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

VIII 评估数据集

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

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

NTU-VIRAL数据集是在南洋理工大学校园使用空中平台收集的，展示了体现独特空中操作挑战的多样化场景。具体来说，“sbs”序列只能从远处物体提供噪声视觉特征。“nya”序列由于半透明表面和复杂的飞行动力学以及低光照条件，对LiDAR SLAM和视觉SLAM都提出了挑战。该数据集配备了10 Hz采样的16通道OS1 gen13 LiDAR和100 Hz的内置IMU，以及两个10 Hz触发的同步针孔相机。评估使用左相机。

Hilti’22和Hilti’23数据集由手持和机器人设备收集，涵盖了来自建筑工地、办公室、实验室和停车场等环境的室内和室外序列。这些序列引入了许多挑战，如长走廊、地下室和楼梯，具有无纹理特征、变化的光照条件和不足的LiDAR平面约束。手持序列使用10 Hz的Hesai PandarXT-325 LiDAR，40 Hz的五个广角相机（降采样为10 Hz），以及400 Hz的外部Bosch BMI085 IMU。同时，机器人安装的序列配备了10 Hz的Robosense BPearl5 LiDAR，10 Hz的八个全向相机，以及200 Hz的Xsens MTi-670 IMU。在这两种情况下，评估所有系统时使用前置相机。每个序列提供了通过运动捕捉系统（MoCap）或全站仪[54]获得的毫米级地面真实值。请注意，Hilti数据集的地面真实值不是开源的；因此，这些数据集上的算法结果通过Hilti官方网站进行评估。由于Hilti’23中的“Site 3”未提供深入的分析图（例如RMSE），我们排除了这四个序列，但这些序列的评分结果仍可在其官方网站6上找到。NTU-VIRAL和Hilti共贡献了25个序列。

MARS-LVIG数据集提供了高空、面向地面的建图数据，涵盖了丛林、山脉和岛屿等多种非结构化地形。该数据集通过DJI M300 RTK四旋翼飞行器收集，配备了Livox Avia7 LiDAR（内置BM1088 IMU）和高分辨率全局快门相机，两者均以10 Hz触发。这与上述NTU-VIRAL和Hilti数据集明显不同，后者使用(752 \times 480)灰度图像，而MARS数据集采用(2448 \times 2048) RGB图像，从而有助于生成清晰、密集的彩色点云。因此，我们利用这个公共数据集来验证我们在高空航空建图应用中的能力。

FAST-LIVO2私有数据集

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

VIII-B1 平台

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

VIII-B2 序列描述

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

IX 实验结果

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

实现和系统配置

我们使用C++和机器人操作系统（ROS）实现了提出的FAST-LIVO2系统。在默认配置中，启用了曝光时间估计，而法线向量细化被关闭。扫描中的LiDAR点以1:3的比例进行时间下采样。体素地图的根体素大小设置为0.5米，内部八叉树的最大层数为3。图像对齐的图像块大小为(8 \times 8)，法线细化的图像块大小为(11 \times 11)。在顺序ESIKF设置中，对于所有实验，相机光度噪声设置为常数100。Livox Avia LiDAR和OS1-16的LiDAR深度误差和方位角误差分别调整为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。对于FAST-LIVO2，我们还在ARM处理器上进行了测试，该处理器通常用于嵌入式系统，具有降低的功耗和成本。ARM平台是RB5®，配备Qualcomm Kryo585 CPU和8 GB RAM。我们将FAST-LIVO2在基于ARM的平台上的实现称为“FAST-LIVO2 (ARM)”。

基准实验

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

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

所有方法的结果如表II所示。可以看出，我们的方法在所有序列中实现了最高的总体准确性，平均RMSE为0.044米，比第二名的FAST-LIVO的0.137米高出三倍。我们的系统在大多数序列中提供了最佳结果，除了“Outside Building”和“Large Room (dark)”，其中我们的系统与仅LiDAR-惯性里程计FAST-LIO2相比显示出略微（毫米级）更高的误差。这种差异可以归因于这些序列中丰富的结构特征但光照条件差，导致图像暗淡和模糊。因此，融合这些低质量图像不会提高里程计的准确性。排除这两个序列，我们的方法，利用紧密耦合的LiDAR、惯性和视觉信息，显著优于FAST-LIO2（我们的LIO子系统）和仅LiDAR-视觉里程计SDV-LOAM。值得注意的是，SDV-LOAM在Hilti数据集上表现特别差，因为它缺乏与IMU测量的紧密集成，导致LO子系统中的漂移。此外，LiDAR和视觉观测之间的松散耦合，以及VO的初始值不佳，经常导致局部最优甚至负优化。我们的LIO子系统通常优于FAST-LIO2，因为我们对每个LiDAR点进行了更准确的噪声建模。在少数FAST-LIO2略优于我们的序列中，差异很小，在毫米级别，可以忽略不计。此外，我们的系统在所有序列中的准确性显著超过其他紧密耦合的LiDAR-惯性-视觉系统。其中，LVI-SAM在九个序列中失败，主要是由于其基于特征的LIO和VIO子系统没有充分利用原始测量，这在具有微妙几何或纹理特征的环境中降低了其鲁棒性。R3LIVE通常表现良好，但在“Construction Stairs”、“Cupola”和“Attic to Upper Gallery”序列中表现不佳，其性能甚至比FAST-LIO2更差。这是因为在无结构的楼梯处的剧烈旋转导致姿态先验不足，导致在将彩色地图点与当前帧对齐时出现局部最优，最终导致负优化。FAST-LIVO和FAST-LIVO2通过基于图像块的对齐克服了这些序列中的挑战。此外，在这些序列中传感器靠近墙壁的情况凸显了FAST-LIVO2中射线投射的有效性，这些大规模场景中的建图结果如补充材料[53]中的图S8所示。另一方面，在NTU-VIRAL数据集上，FAST-LIVO的表现不如R3LIVE和FAST-LIVO2，特别是在“nya”序列等非结构化场景中，基于恒定深度假设的仿射变换效果不准确。相比之下，R3LIVE的像素级对齐和FAST-LIVO2的平面先验（或细化）不会遇到这样的问题。

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

LiDAR退化和视觉挑战环境

在本实验中，我们评估了系统在LiDAR退化和/或视觉挑战环境下的鲁棒性，将其与FAST-LIVO和R3LIVE在8个序列中的定性建图结果进行比较，如图10和11所示。图10展示了LiDAR退化序列，其中LiDAR面对一堵大墙，同时沿墙从一侧移动到另一侧。由于LiDAR仅观察到一个墙平面，缺乏几何约束，LIO方法会失败。值得一提的是，“HIT Graffiti Wall”序列持续近800米，LiDAR持续面对墙壁，导致严重的退化。在所有序列中，FAST-LIVO2明显展示了其对长期退化的鲁棒性及其提供高精度彩色点地图的能力。相比之下，FAST-LIVO获得了几何结构但纹理完全模糊。R3LIVE在几何结构和纹理清晰度上都表现不佳。图11展示了在更复杂场景中的测试，其中LiDAR和/或相机偶尔退化。退化方向由相应的箭头指示。“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©）仅显示FAST-LIVO2结果，因为R3LIVE和FAST-LIVO失败。在图11(b)中，蓝色箭头表示朝向纯黑色屏幕的移动，表示LiDAR和相机同时退化。在图11(c1)和(c2)中，红点表示该位置的LiDAR扫描，说明了由于观察到的单一平面导致的LiDAR退化区域。此外，“Mining Tunnel”在整个序列中光照非常暗淡，伴随着频繁的视觉和LiDAR退化。尽管存在这些挑战，FAST-LIVO2在两个序列中仍以小于0.01米的端到端误差返回起点。

高精度建图

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

运行时间分析

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

FAST-LIVO2相比前身FAST-LIVO也有显著改进。主要的改进源于我们在稀疏图像对齐中应用了逆组合公式。基于LiDAR点的平面先验的仿射变换进一步提高了我们方法的收敛效率。因此，FAST-LIVO2将每个金字塔级别的迭代次数从10次减少到3次，同时仍实现了更高的准确性。

X 应用

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

完全机载自主无人机导航

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

X-A1 系统配置

硬件和软件设置如图12所示。硬件方面，我们使用NUC（Intel i7-1360P CPU和32 GB RAM）作为机载计算机。软件方面，定位组件由FAST-LIVO2提供，以10Hz的频率提供位置反馈。定位结果反馈给飞行控制器，以实现位置、速度和姿态的200Hz反馈。除了定位，FAST-LIVO2还向规划模块Bubble planner [55]提供密集的注册点云，该模块规划一条平滑的轨迹，然后由流形上的模型预测控制（MPC）[56]跟踪。MPC计算期望的角速度和推力，由运行在飞行控制器上的低级角速率控制器跟踪。重要的是，MPC、规划器和FAST-LIVO2都在机载计算机上实时运行。

X-A2 无人机自主导航

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

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

空中建图

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

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

利用从FAST-LIVO2获得的高精度传感器定位和密集3D彩色点地图，我们开发了用于渲染管线的软件应用，包括网格化和纹理化，以及新兴的NeRF类渲染管线，如3D高斯泼溅（3DGS）。对于网格化，我们在“CBD Building 01”中基于截断符号距离函数（TSDF）使用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厘米），引入了更多需要优化的参数。尽管如此，我们点云的密度和精度的增加导致与从COLMAP输入获得的PSNR相比，PSNR略高。

XI 结论与未来工作

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

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

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

参考文献

[1] R. Mur-Artal and J. D. Tardos, “Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras,” IEEE transactions on robotics, vol. 33, no. 5, pp. 1255-1262, 2017.

[2] J. Engel, V. Koltun, and D. Cremers, “Direct sparse odometry,” IEEE transactions on pattern analysis and machine intelligence, vol. 40, no. 3, pp. 611-625, 2017.

[3] J. Engel, T. Schops, and D. Cremers, “Lsd-slam: Large-scale direct monocular slam,” in European conference on computer vision. Springer, 2014, pp. 834-849.

[4] C. Forster, Z. Zhang, M. Gassner, M. Werlberger, and D. Scaramuzza, “Svo: Semidirect visual odometry for monocular and multicamera systems,” IEEE Transactions on Robotics, vol. 33, no. 2, pp. 249-265, 2016.

[5] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time.” in Robotics: Science and Systems, vol. 2, no. 9, 2014.

[6] J. Lin and F. Zhang, “Loam livox: A fast, robust, high-precision lidar odometry and mapping package for lidars of small fov,” in 2020 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2020, pp. 3126-3131.

[7] T. Shan and B. Englot, “Lego-loam: Lightweight and ground-optimized lidar odometry and mapping on variable terrain,” in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 4758-4765.

[8] C. Zheng, Q. Guo, W. Xu, X. Liu, Q. Guo, and F. Zhang, “Fast-livo: Fast and tightly-coupled sparse-direct lidar-inertial-visual odometry,” in 2022 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2022, pp. 4003-4009.

[9] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004-1020, 2018.

[10] R. Mur-Artal, J. M. M. Montiel, and J. D. Tardos, “Orb-slam: a versatile and accurate monocular slam system,” IEEE transactions on robotics, vol. 31, no. 5, pp. 1147-1163, 2015.

[11] M.Irani and P.Anandan, “All about direct methods,” in Proc. Workshop Vis. Algorithms, Theory Pract, 1999, pp. 267-277.

[12] C. Forster, M. Plzodi, and D. Scaramuzza, “Svo: Fast semi-direct monocular visual odometry,” in 2014 IEEE international conference on robotics and automation (ICRA). IEEE, 2014, pp. 15-22.

[13] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, “Fast-lio2: Fast direct lidar-inertial odometry,” IEEE Transactions on Robotics, pp. 1-21, 2022.

[14] C. Yuan, W. Xu, X. Liu, X. Hong, and F. Zhang, “Efficient and probabilistic adaptive voxel mapping for accurate online lidar odometry,” IEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 8518-8525, 2022.

[15] M. Meilland, A. I. Comport, and P. Rives, “Real-time dense visual tracking under large lighting variations,” in British Machine Vision Conference.
British Machine Vision Association, 2011, pp. 45–1.

[16] T. Tykk¨
al¨
a, C. Audras, and A. I. Comport, “Direct iterative closest point
for real-time visual odometry,” in 2011 IEEE International Conference
on Computer Vision Workshops (ICCV Workshops).
IEEE, 2011, pp.
2050–2056.

[17] C. Kerl, J. Sturm, and D. Cremers, “Robust odometry estimation for
rgb-d cameras,” in 2013 IEEE international conference on robotics and
automation.
IEEE, 2013, pp. 3748–3754.

[18] J. Engel, J. Sturm, and D. Cremers, “Semi-dense visual odometry for a
monocular camera,” in Proceedings of the IEEE international conference
on computer vision, 2013, pp. 1449–1456.

[19] K. Chen, R. Nemiroff, and B. T. Lopez, “Direct lidar-inertial odometry:
Lightweight lio with continuous-time motion correction,” in 2023 IEEE
International Conference on Robotics and Automation (ICRA).
IEEE,
2023, pp. 3983–3989.

[20] Z. Wang, L. Zhang, Y. Shen, and Y. Zhou, “D-liom: Tightly-coupled
direct lidar-inertial odometry and mapping,” IEEE Transactions on
Multimedia, 2022.

[21] J. Zhang and S. Singh, “Laser–visual–inertial odometry and mapping
with high robustness and low drift,” Journal of field robotics, vol. 35,
no. 8, pp. 1242–1264, 2018.

[22] W. Shao, S. Vijayarangan, C. Li, and G. Kantor, “Stereo visual inertial
lidar simultaneous localization and mapping,” in 2019 IEEE/RSJ Inter-
national Conference on Intelligent Robots and Systems (IROS).
IEEE,
2019, pp. 370–377.

[23] J. Zhang, M. Kaess, and S. Singh, “A real-time method for depth
enhanced visual odometry,” Autonomous Robots, vol. 41, pp. 31–43,
2017.

[24] J. Graeter, A. Wilczynski, and M. Lauer, “Limo: Lidar-monocular visual
odometry,” in 2018 IEEE/RSJ international conference on intelligent
robots and systems (IROS).
IEEE, 2018, pp. 7872–7879.

[25] Y. Zhu, C. Zheng, C. Yuan, X. Huang, and X. Hong, “Camvox: A
low-cost and accurate lidar-assisted visual slam system,” in 2021 IEEE
International Conference on Robotics and Automation (ICRA).
IEEE,
2021, pp. 5049–5055.

[26] S.-S. Huang, Z.-Y. Ma, T.-J. Mu, H. Fu, and S.-M. Hu, “Lidar-
monocular visual odometry using point and line features,” in 2020 IEEE
International Conference on Robotics and Automation (ICRA).
IEEE,
2020, pp. 1091–1097.

[27] C. Campos, R. Elvira, J. J. G. Rodr´
ıguez, J. M. Montiel, and J. D.
Tard´
os, “Orb-slam3: An accurate open-source library for visual, visual–
inertial, and multimap slam,” IEEE Transactions on Robotics, vol. 37,
no. 6, pp. 1874–1890, 2021.

[28] Y.-S. Shin, Y. S. Park, and A. Kim, “Dvl-slam: Sparse depth enhanced
direct visual-lidar slam,” Autonomous Robots, vol. 44, no. 2, pp. 115–
130, 2020.

[29] X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, “Lic-fusion: Lidar-
inertial-camera odometry,” in 2019 IEEE/RSJ International Conference
on Intelligent Robots and Systems (IROS), 2019, pp. 5848–5854.

[30] K. Sun, K. Mohta, B. Pfrommer, M. Watterson, S. Liu, Y. Mulgaonkar,
C. J. Taylor, and V. Kumar, “Robust stereo visual inertial odometry for
fast autonomous flight,” IEEE Robotics and Automation Letters, vol. 3,
no. 2, pp. 965–972, 2018.

[31] X. Zuo, Y. Yang, P. Geneva, J. Lv, Y. Liu, G. Huang, and M. Pollefeys,
“Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window
plane-feature tracking,” in 2020 IEEE/RSJ International Conference on
Intelligent Robots and Systems (IROS).
IEEE, 2020, pp. 5112–5119.

[32] D. Wisth, M. Camurri, S. Das, and M. Fallon, “Unified multi-modal
landmark tracking for tightly coupled lidar-visual-inertial odometry,”
IEEE Robotics and Automation Letters, vol. 6, no. 2, pp. 1004–1011,
2021.

[33] J. Lin, C. Zheng, W. Xu, and F. Zhang, “R2live: A robust, real-time,
lidar-inertial-visual tightly-coupled state estimator and mapping,” IEEE
Robotics and Automation Letters, vol. 6, no. 4, pp. 7469–7476, 2021.

[34] C. F. W. Bell B.M., “The iterated kalman filter update as a gauss-newton
method,” Automatic Control IEEE Transactions, vol. 38, no. 2, pp. 294–
297, 1993.

[35] T. Shan, B. Englot, C. Ratti, and D. Rus, “Lvi-sam: Tightly-coupled
lidar-visual-inertial odometry via smoothing and mapping,” in 2021
IEEE international conference on robotics and automation (ICRA).
IEEE, 2021, pp. 5692–5698.

[36] J. Lin and F. Zhang, “R 3 live: A robust, real-time, rgb-colored, lidar-
inertial-visual tightly-coupled state estimation and mapping package,”
in 2022 International Conference on Robotics and Automation (ICRA).
IEEE, 2022, pp. 10 672–10 678.

[37] ——, “R 3 live++: A robust, real-time, radiance reconstruction pack-
age with a tightly-coupled lidar-inertial-visual state estimator,” arXiv
preprint arXiv:2209.03666, 2022.

[38] J. Engel, V. Usenko, and D. Cremers, “A photometrically cali-
brated benchmark for monocular visual odometry,” arXiv preprint
arXiv:1607.02555, 2016.

[39] W. Wang, J. Liu, C. Wang, B. Luo, and C. Zhang, “Dv-loam: Direct
visual lidar odometry and mapping,” Remote Sensing, vol. 13, no. 16,
p. 3340, 2021.

[40] Z. Yuan, Q. Wang, K. Cheng, T. Hao, and X. Yang, “Sdv-loam:
Semi-direct visual-lidar odometry and mapping,” IEEE Transactions on
Pattern Analysis and Machine Intelligence, 2023.

[41] H. Zhang, L. Du, S. Bao, J. Yuan, and S. Ma, “Lvio-fusion:tightly-
coupled lidar-visual-inertial odometry and mapping in degenerate envi-
ronments,” IEEE Robotics and Automation Letters, vol. 9, no. 4, pp.
3783–3790, 2024.

[42] W. Xu and F. Zhang, “Fast-lio: A fast, robust lidar-inertial odometry
package by tightly-coupled iterated kalman filter,” IEEE Robotics and
Automation Letters, pp. 1–1, 2021.

[43] D. He, W. Xu, and F. Zhang, “Symbolic representation and toolkit de-
velopment of iterated error-state extended kalman filters on manifolds,”
IEEE Transactions on Industrial Electronics, 2023.

[44] D. Willner, C.-B. Chang, and K.-P. Dunn, “Kalman filter algorithms
for a multi-sensor system,” in 1976 IEEE conference on decision and
control including the 15th symposium on adaptive processes.
IEEE,
1976, pp. 570–574.

[45] J. Ma and S. Sun, “Globally optimal distributed and sequential state fu-
sion filters for multi-sensor systems with correlated noises,” Information
Fusion, p. 101885, 2023.

[46] Y. Ren, Y. Cai, F. Zhu, S. Liang, and F. Zhang, “Rog-map: An efficient
robocentric occupancy grid map for large-scene and high-resolution
lidar-based motion planning,” arXiv preprint arXiv:2302.14819, 2023.

[47] R. M. Stereopsis, “Accurate, dense, and robust multiview stereopsis,”
IEEE TRANSACTIONS ON PATTERN ANALYSIS AND MACHINE IN-
TELLIGENCE, vol. 32, no. 8, 2010.

[48] S. Baker and I. Matthews, “Lucas-kanade 20 years on: A unifying
framework,” International journal of computer vision, vol. 56, pp. 221–
255, 2004.

[49] T.-M. Nguyen, S. Yuan, M. Cao, Y. Lyu, T. H. Nguyen, and L. Xie,
“Ntu viral: A visual-inertial-ranging-lidar dataset, from an aerial vehicle
viewpoint,” The International Journal of Robotics Research, vol. 41,
no. 3, pp. 270–280, 2022.

[50] M. Helmberger, K. Morin, B. Berner, N. Kumar, G. Cioffi, and
D. Scaramuzza, “The hilti slam challenge dataset,” IEEE Robotics and
Automation Letters, vol. 7, no. 3, pp. 7518–7525, 2022.

[51] L. Zhang, M. Helmberger, L. F. T. Fu, D. Wisth, M. Camurri, D. Scara-
muzza, and M. Fallon, “Hilti-oxford dataset: A millimeter-accurate
benchmark for simultaneous localization and mapping,” IEEE Robotics
and Automation Letters, vol. 8, no. 1, pp. 408–415, 2022.

[52] H. Li, Y. Zou, N. Chen, J. Lin, X. Liu, W. Xu, C. Zheng, R. Li, D. He,
F. Kong, et al., “Mars-lvig dataset: A multi-sensor aerial robots slam
dataset for lidar-visual-inertial-gnss fusion,” The International Journal
of Robotics Research, p. 02783649241227968, 2024.

[53] “Supplementary material: Fast-livo2: Fast, direct lidar-inertial-visual
odometry,” available online: https://github.com/hku-mars/FAST-LIVO2/
blob/main/Supplementary/LIVO2 supplementary.pdf.

[54] C. Klug, C. Arth, D. Schmalstieg, and T. Gloor, “Measurement uncer-
tainty analysis of a robotic total station simulation,” in IECON 2018-44th
Annual Conference of the IEEE Industrial Electronics Society.
IEEE,
2018, pp. 2576–2582.

[55] Y. Ren, F. Zhu, W. Liu, Z. Wang, Y. Lin, F. Gao, and F. Zhang,
“Bubble planner: Planning high-speed smooth quadrotor trajectories
using receding corridors,” in 2022 IEEE/RSJ International Conference
on Intelligent Robots and Systems (IROS). IEEE, 2022, pp. 6332–6339.

[56] G. Lu, W. Xu, and F. Zhang, “On-manifold model predictive control
for trajectory tracking on robotic systems,” IEEE Transactions on
Industrial Electronics, vol. 70, no. 9, pp. 9192–9202, 2022.

[57] I. Vizzo, T. Guadagnino, J. Behley, and C. Stachniss, “Vdbfusion:
Flexible and efficient tsdf integration of range sensor data,” Sensors,
vol. 22, no. 3, 2022. [Online]. Available: https://www.mdpi.com/
1424-8220/22/3/1296

[58] D. Cernea, “Openmvs: multi-view stereo reconstruction library. 2020,”
URL: https://cdcseacave. github. io/openMVS, vol. 5, no. 6, p. 7, 2020.

[59] J. L. Sch¨
onberger and J.-M. Frahm, “Structure-from-motion revisited,”
in 2016 IEEE Conference on Computer Vision and Pattern Recognition
(CVPR), 2016, pp. 4104–4113.