R3 LIVE：一个强大、实时、RGB彩色、激光雷达-惯性-视觉紧密耦合的状态估计和建图

R3 LIVE：一个强大、实时、RGB彩色、激光雷达-惯性-视觉紧密耦合的状态估计和建图

Jiarong Lin and Fu Zhang

摘要

        在本论文中，我们提出了一种新颖的激光雷达-惯性-视觉传感器融合框架，命名为R3 LIVE，利用激光雷达、惯性和视觉传感器的测量来实现强大而准确的状态估计。R3 LIVE包含两个子系统，即激光雷达-惯性里程计（LIO）和视觉-惯性里程计（VIO）。LIO子系统（FASTLIO）利用来自激光雷达和惯性传感器的测量数据构建全局地图的几何结构（即3D点的位置）。VIO子系统利用视觉-惯性传感器的数据渲染地图的纹理（即3D点的颜色）。具体而言，VIO子系统通过最小化帧与地图的光度误差，直接有效地融合视觉数据。我们开发的R3 LIVE系统基于我们之前的工作R2 LIVE，经过精心的架构设计和实施。实验结果表明，所得系统在状态估计方面比当前的对手更具鲁棒性和更高的精度（请参阅附带的视频 1 ）。

        R3 LIVE是一个多才多艺且精心设计的系统，可用于各种可能的应用，不仅可以用作实时机器人应用的SLAM系统，还可以用于像测绘和制图等应用中重建密集、精确、RGB彩色的3D地图。此外，为了使R3 LIVE更具可扩展性，我们开发了一系列离线工具，用于重建和纹理化网格，进一步缩小了R3 LIVE与各种3D应用（如模拟器、视频游戏等）之间的差距（请参阅我们的演示视频 ^2 ）。为了分享我们的发现并为社区做出贡献，我们在我们的Github上开源了R3 LIVE，包括我们所有的代码、软件工具以及我们设备的机械设计。

1. 引言

        近年来，LiDAR传感器在各种机器人应用中越来越广泛地使用，如自动驾驶汽车[1]、无人机[2]-[4]等。特别是随着低成本固态LiDAR（例如[5]）的出现，基于这些LiDAR的应用[6]-[10]推动了机器人社区的发展。然而，对于基于LiDAR的SLAM系统，它们很容易在缺乏足够几何特征的情况下失败，特别是对于通常具有有限视场（FoV）的固态LiDAR[11]。为解决这个问题，将LiDAR与其他传感器（如摄像头[12]-[15]和超宽带（UWB）[16,17]）融合可以提高系统的鲁棒性和准确性。特别是最近在机器人社区中提出了各种LiDAR-视觉融合框架[18]。

图1：(a) 我们开发的R3 LIVE能够实时重建环境的密集、3D、RGB彩色点云。白色路径表示我们用于收集数据的行进轨迹。 (b) 离线重建的网格，网格在(c)中进行了纹理渲染。

        在初始化LiDAR映射子系统时，使用了松耦合的视觉-惯性里程计（VIO）作为运动模型。类似地，在[20]中，作者提出了一种立体视觉-惯性激光雷达SLAM，它将紧密耦合的立体视觉-惯性里程计与LiDAR映射和LiDAR增强视觉闭环结合在一起。最近，王等人提出了DV-LOAM [21]，这是一个直接的视觉-LiDAR融合框架。系统首先利用两阶段的直接视觉里程计模块进行高效的粗略状态估计，然后使用LiDAR映射模块对粗略位姿进行优化，最后使用环路闭合模块来校正累积的漂移。上述系统以松耦合的方式融合了LiDAR-惯性-视觉传感器，其中LiDAR测量没有与视觉或惯性测量一起进行联合优化。

        最近提出了更紧密耦合的LiDAR-惯性-视觉融合框架。例如，左等人提出的LIC-fusion [14]是一个紧密耦合的LiDAR-惯性-视觉融合框架，它在Multi-State Constrained Kalman Filter（MSCKF）框架内结合了IMU测量、稀疏视觉特征和LiDAR特征的在线空间和时间校准。为了进一步增强LiDAR扫描匹配的鲁棒性，他们的后续工作LIC-Fusion 2.0 [15]提出了一种跨多个LiDAR扫描的平面特征跟踪算法，通过滑动窗口在窗口内对姿态轨迹进行优化。在[13]中，单等人提出了LVI-SAM，通过一个建立在因子图之上的紧密耦合的平滑和映射框架，将LiDAR-视觉-惯性传感器融合在一起。LVISAM的LiDAR-惯性和视觉-惯性子系统可以在检测到其中一个失败时独立运行，或者在检测到足够多的特征时联合运行。我们之前的工作R2 LIVE [12]紧密融合了LiDAR-惯性-视觉传感器的数据，提取了LiDAR和稀疏视觉特征，通过最小化特征重投影误差，在迭代卡尔曼滤波的误差状态框架内估计状态，以实现实时性能，同时通过滑动窗口优化提高了整体视觉映射的准确性。R2 LIVE能够在各种具有激烈运动、传感器故障甚至在有大量移动物体和小视场（LiDAR FoV）的狭窄隧道环境中运行。

        在本文中，我们解决了基于紧密耦合的LiDAR、惯性和视觉测量融合的实时同时定位、3D地图制作和地图渲染的问题。我们的贡献如下：

• 我们提出了一个实时的同时定位、制图和着色框架。该框架由用于重建几何结构的激光雷达-惯性里程计（LIO）和用于纹理渲染的视觉-惯性里程计（VIO）组成。整个系统能够实时重建环境的密集、3D、RGB彩色点云（见图1（a））。

• 我们提出了一种基于RGB彩色点云地图的新颖VIO系统。VIO通过最小化观察到的地图点的RGB颜色与当前图像中的测量颜色之间的光度误差来估计当前状态。这个过程不需要环境中明显的视觉特征，并且节省了相应的处理时间（例如特征检测和提取），这使得我们提出的系统在特征较少的环境中尤其具有鲁棒性。

• 我们将提出的方法实现为一个完整的系统，R3 LIVE，能够在实时且低漂移的情况下构建密集、精确、3D、RGB彩色点云地图。整个系统在各种室内和室外环境中进行了验证。结果显示，在行驶了1.5公里后，我们的系统仅出现0.16米的平移漂移和3.9度的旋转漂移。

• 我们在Github上开源了我们的系统。我们还开发了一些离线工具，用于从彩色点云中重建和添加纹理到网格中（见图1（b）和（c））。这些软件工具和我们设备的机械设计也是开源的，以造福可能的应用领域。

2. 系统概述

        我们系统的概述如图2所示，我们提出的框架包含两个子系统：LIO子系统（上部分）和VIO子系统（下部分）。

        LIO子系统构建了一个全局地图的几何结构，它注册输入的激光雷达扫描，并通过最小化点到平面的残差来估计系统的状态。VIO子系统构建地图的纹理，它使用输入图像为每个点渲染RGB颜色，并通过最小化帧与帧之间的PnP重投影误差和帧与地图的光度误差来更新系统状态。

图2：我们提出的系统概述

 3. 符号术语

        在本文中，我们使用表格I中介绍的符号，这些符号已经在之前的工作R^2 LIVE [12]中引入过。

表I：术语表

 A. 状态向量

        在我们的工作中，我们定义完整的状态向量为：

        其中 是以全局坐标系表示的重力向量（即第一个激光雷达坐标系），是IMU和相机之间的时间偏移，假设激光雷达已经与IMU同步，是相机内参数向量，其中 是像素焦距，是主点相对于图像平面左上角的偏移。

B. 地图表示

        我们的地图由体素（voxels）和点组成，其中点包含在体素内并且是地图的最小元素。

        1. 体素：为了在我们的VIO子系统中快速查找地图中的点以进行渲染和跟踪（请参见第V-C节和第V-D节），我们设计了一个固定大小（例如0.1m×0.1m×0.1m）的容器，称为体素。如果一个体素最近（例如在最近1秒内）添加了点，我们将此体素标记为已激活。否则，将此体素标记为未激活。

        2.Points: 在我们的工作中，一个点P是一个大小为6的向量：

其中，第一个子向量  表示全局坐标系中的点的三维位置，第二个向量表示点的RGB颜色。此外，我们还记录了该点的其他信息，如协方差矩阵和 Σ_c，分别表示了和的估计误差，以及该点被创建和渲染的时间戳。

4. 激光雷达-惯性里程计子系统（LIDAR-INERTIAL ODOMETRY SUBSYSTEM）

        如图2所示， LIVE的LIO子系统构建了全局地图的几何结构。对于传入的激光雷达扫描，由于帧内的连续运动而产生的运动畸变通过IMU的反向传播进行补偿，如[6]所示。然后，我们利用误差状态迭代卡尔曼滤波器（ESIKF）最小化点到平面的残差来估计系统的状态。最后，在收敛的状态下，将该扫描的点附加到全局地图上，并标记相应的体素为激活或未激活状态。全局地图中累积的3D点形成了几何结构，还用于为我们的VIO子系统提供深度信息。有关 LIVE中LIO子系统的详细实现，请参考我们先前的相关工作[12, 22]。

5. 视觉-惯性里程计子系统（VISUAL-INERTIAL ODOMETRY SUBSYSTEM）

        我们的VIO子系统渲染全局地图的纹理，并通过最小化光度误差来估计系统状态。更具体地说，我们将一定数量的点（即跟踪点）从全局地图投影到当前图像中，然后通过最小化这些点的光度误差，在ESIKF框架内迭代估计系统状态。为了提高效率，跟踪的地图点通常是稀疏的，这通常需要构建输入图像的金字塔[^4]。然而，金字塔既不对平移也不对旋转不变，需要额外的估计。在我们提出的框架中，我们利用单个地图点的颜色来计算光度误差。颜色是在VIO中同时渲染的地图点的固有属性，并且不受相机的平移和旋转的影响。为了确保鲁棒性和快速收敛，我们设计了如图2所示的两步流程。首先，我们利用帧与帧的光流跟踪地图点，并通过最小化跟踪地图点的透视n点（PnP）投影误差来优化系统状态（第V-A节）。然后，我们通过最小化跟踪点之间的帧到地图光度误差进一步细化系统状态估计(第V-B节)。使用收敛的状态估计和原始输入图像，我们执行纹理渲染来更新全局地图中点的颜色（第V-C节）。

 图3：我们的帧对帧ESIKF VIO通过最小化PnP投影误差来更新系统状态

A. 帧对帧视觉-惯性里程计

        假设我们在上一帧图像中跟踪到了m个地图点，它们在上的投影是，我们利用Lucas-Kanade光流方法来找出它们在当前图像帧中的位置，表示为。然后，我们计算并优化它们的投影误差，以通过ESIKF来估计状态。

        1）透视n点投影误差：如图3所示，我们以第个点为例，投影误差计算如下：

        其中，是每次ESIKF迭代中的当前状态估计，的计算如下：

 

        其中， 是上一帧图像和当前图像帧之间的时间间隔。请注意，在式(5)中，第一项是针孔投影函数，第二项是在线时间校正因子 [23]。

        在残差项(4)中的测量噪声包含两个来源：一个是中的像素跟踪误差，另一个是地图点位置误差 。

 

        其中，  和分别是和的真实值。然后，我们获得了真实零残差的一阶泰勒展开如下：

        其中， a并且

        2）帧对帧VIO ESIKF更新：方程（8）构成了的观测分布（或等效地，），可以与来自IMU传播的先验分布结合，以获得的最大后验（MAP）估计：

        其中，是带有协方差Σ的平方马哈拉诺比斯距离，是IMU传播的状态估计，是IMU传播的状态协方差，H是将状态误差从的切空间投影到 的切空间时的雅可比矩阵。关于式（12）中第一项的详细推导可以在 LIVE [12]的第E节中找到。

标记为：

        根据[6]，我们计算卡尔曼增益如下：

        然后，我们可以更新状态估计如下：

        上述过程（第V-A1节到第V-A2节）迭代直到收敛（即，更新小于给定的阈值）。请注意，这样的迭代卡尔曼滤波器被认为等价于高斯-牛顿优化[24]。

B. 帧到地图视觉-惯性里程计

        1） 帧到地图光度更新：在帧对帧VIO更新之后，我们有一个良好的估计状态，然后通过最小化跟踪点的光度误差来执行帧到地图VIO更新，以降低漂移。如图4所示，以第s个跟踪点为例，它的帧到地图光度误差计算如下：

        其中，是保存在全局地图中的点颜色，是在当前图像Ik中观测到的颜色。为了获得观测颜色及其协方差)，我们预测点的位置在当前图像上，然后线性插值邻近像素的RGB颜色。

图4：我们的帧到地图ESIKF VIO通过最小化地图点颜色与当前图像中的测量颜色之间的光度误差来更新系统状态。

        我们还考虑和的测量噪声：

        其中，是的真实值，是的真实值，是当前帧和上次渲染时间之间的时间间隔。请注意，在式（21）中，的测量噪声包含两部分：来自上次渲染的估计误差（见第V-C节），以及脉冲随机行走过程噪声，用于解释环境错觉的变化。

        结合（19）、（20）和（21），我们得到真实零残差o的一阶泰勒展开：

        其中β，

        2） 帧到地图VIO ESIKF更新：方程（22）构成了另一个用于的观测分布，它与来自IMU传播的先验分布结合，以获得的最大后验（MAP）估计：

记和类似于（13）（16）：

图5：我们通过贝叶斯更新渲染点c_s的颜色

        然后，我们执行类似于（17）和（18）的状态更新。这个帧到地图VIO ESIKF更新（第V-B1节到第V-B2节）迭代直到收敛。收敛的状态估计然后用于：（1）渲染地图的纹理（第V-C节）；（2）更新下一帧要使用的当前跟踪点集（第V-D节）；（3）作为下一帧LIO或VIO更新中IMU传播的起点。

C 渲染全局地图的纹理

         在帧到地图VIO更新之后，我们有了当前图像的精确姿态，然后执行渲染函数来更新地图点的颜色。

首先，我们检索所有激活的体素中的点（见第IV节）。假设点的总数是 points，点集为​​​​​​​。以第个点的颜色更新过程为例。如果位于当前图像中，我们通过线性插值上邻近像素的RGB值来获取其观测颜色和协方差。新观测的点颜色通过贝叶斯更新（见图5）与地图中记录的现有颜色融合，导致更新后的颜色和的相关协方差如下：

D VIO子系统跟踪点的更新

        在纹理渲染之后，我们对跟踪点集P进行更新。首先，如果它们在（4）中的投影误差或在（19）中的光度误差太大，我们将从P中移除这些点，并且也将不在中的点移除。其次，我们将ζ中的每个点投影到当前图像I_k上，并且如果附近没有其他跟踪点（例如，在50个像素的半径内），则将其添加到中。

图6：我们的数据采集手持设备，（a）显示了我们的最小系统，总重量为2.09公斤；（b）额外使用了D-GPS RTK系统和ArUco标记板[25]来评估系统的精度。

5 实验和结果

A. 设备设置

         我们的数据采集手持设备如图6(a)所示，包括电源供应单元、机载DJI manifold-计算平台（配备Intel i78550u CPU和8 GB RAM）、FLIR Blackfly BFS-u3-13y3c全局快门相机和 LiDAR。相机的视场为，而LiDAR的视场为。为了定量评估我们算法的精度（第VI-D节），我们在设备上安装了差分GPS（DGPS）实时运动学（RTK）系统^7，如图6(b)所示。在GPS受限环境中（第VI-B节和第VI-C节），我们使用ArUco标记[25]来计算我们里程计的漂移。请注意，这个设备的所有机械模块都设计为FDM可打印8，为了便于我们的读者重现我们的工作，我们在我们的github9上开源了所有的原理图。

B. 实验1：在同时存在LiDAR退化和视觉无纹理环境中进行鲁棒性评估

在这个实验中，我们挑战了SLAM中最困难的场景之一，即在同时存在LiDAR退化和视觉无纹理环境中进行鲁棒性评估。如图7所示，我们的传感器穿过一个狭窄的“T”形通道，偶尔会面对侧壁。当面对只施加单平面约束的壁面时，众所周知，LiDAR对于完整姿态估计是退化的。同时，白色墙壁上的视觉纹理非常有限（图7(a)和图7(c)），尤其是壁面-1，它只有照明的变化。这样的情景对于基于LiDAR和基于视觉的SLAM方法都是具有挑战性的。

利用原始图像像素颜色信息，我们提出的算法可以在这种极端困难的情况下“生存”。图8显示了我们估计的姿态，通过“墙-1”和“墙-2”的通过阶段用蓝色和黄色阴影标示。估计的协方差也显示在图8中，它在整个估计轨迹上都有界，表明我们的估计质量在整个过程中是稳定的。传感器移动到起始点，在那里使用ArUco标记板获取起始和结束姿态之间的地面真实相对姿态。与地面真实的结束姿态相比，我们的算法在旋转方向上漂移1.62∘，在平移方向上漂移4.57 cm.

 图7：我们在同时存在LiDAR退化和视觉无纹理情景中测试我们的算法

图8：实验1的估计姿势及其边界，放大了5倍以更好地可视化（轨迹周围的浅色区域）。蓝色和黄色的阴影区域分别表示传感器面对白色“墙壁-1”和“墙壁2”的阶段。最终，算法在旋转方面漂移了1.62°，在平移方面漂移了4.57厘米。

C 实验2：高精度地图大规模室内和室外城市环境

        在这个实验中，我们展示了我们的算法在实时重建大规模校园环境的精确、密集、3D、RGB彩色地图的能力。我们在香港科技大学（HKUST）校园内收集了4次不同行进轨迹（即轨迹1-4）的数据，它们的总长度分别为1317、1524、1372和1191米。这些轨迹的俯视图（即投影在X-Y平面上）如图10所示，它们的高度变化如图11所示。在没有任何额外处理（例如回环）的情况下，这四条轨迹都可以自行闭合（见图9（e））。使用放置在起点的ArUco标记板，里程漂移如表II所示，表明我们提出的方法在长时间轨迹和复杂环境中具有高精度，漂移很小。最后，我们在图9中呈现了“轨迹1”中的重建地图。更多的可视化结果可以在项目页面上找到12。

表II：实验2的4条轨迹中的里程漂移

	Traj-1	Traj-2	Traj-3	Traj-4
Length of trajectory (m)	1317	1524	1372	1191
Drift in translation (m)	0.093	0.154	0.164	0.102
Drift in rotation (deg)	2.140	0.285	2.342	3.925

D 实验3：使用D-GPS RTK定量评估精度

        在这个实验中，我们使用实时差分GPS（D-GPS）运动学（RTK）系统在一个海港（Belcher Bay Promenade 13 ）中收集了两个数据序列，提供了地面真实轨迹。在这两个序列中，传感器经常面对许多行人，偶尔面对开阔的海洋，其中激光雷达的测量很少。

        我们将R3 LIVE的两种不同配置（“R3LIVE-HiRES”和“R3LIVE-RT”）的估计轨迹与“LVI-SAM”（对其Livox Avia激光雷达进行了修改），以及“R2 LIVE” [12]，“VINSMono”（IMU+相机）[26]，“Fast-LIO2”（IMU+激光雷达）[22]进行了比较，地面真实轨迹如图12所示，在这里我们可以看到我们的估计轨迹在两个序列中都与地面真实轨迹最吻合。为了进行更多的定量比较，我们计算了所有可能的长度（50,100,150,…,300）米的子序列的相对旋转误差（RPE）和相对平移误差（RTE）[27]，如表III所示。

E. 运行时间分析

        我们在两个不同的平台上（台式电脑和无人机机载计算机）对我们系统在所有实验中的平均时间消耗进行了调查：台式电脑（配备Intel i7-9700K CPU和32GB RAM）和无人机机载计算机（“OB”，配备Intel i7-8550u CPU和8GB RAM）。详细的统计数据列在表IV中。我们的VIO子系统的时间消耗受到两个主要设置的影响：图像分辨率和点云地图分辨率（“Pt_res”）。

5 R3 LIVE的应用

A. 网格重建和纹理处理

        虽然R3 LIVE可以实时重建彩色的3D地图，但我们还开发了用于离线网格重建和纹理处理的软件实用程序（见图13）。对于网格构建，我们利用在CGAL [29]中实现的Delaunay三角剖分和图割 [28]。在进行网格构建后，我们使用由我们的VIO子系统渲染的顶点颜色对网格进行纹理处理。

        我们开发的实用程序还可以将R3 LIVE中的彩色点地图或离线网格化地图导出为常用文件格式，如“pcd”15、“ply”16、“obj”等。因此，由R3 LIVE重建的地图可以被导入各种3D软件应用程序，包括但不限于CloudCompare [30]、Meshlab [31]、AutoDesk 3ds Max 等。

图9：我们展示了在实验2中实时重建的稠密3D RGB彩色地图。在（a）中，我们将我们的地图与GoogleEarth卫星图像合并，发现它们对齐良好。地图的细节分别在（b）、（c）和（d）中有选择地显示。在（e）中，我们展示了我们的算法可以自行关闭循环，无需任何额外的处理（例如，循环检测和闭合），返回轨迹可以返回到起点。我们建议读者查看我们在Youtube上的视频以获取更多详细信息。

图10：实验2中4条轨迹的俯视图，这四条轨迹的总长度分别为1317米、1524米、1372米和1191米。

图11：实验2中4条轨迹的高度变化，其中时间标度标准化为1。

图12：实验3中估计轨迹的比较。

表格III：实验3中的相对姿势误差。在“R3LIVE-HiRES”配置中，我们将输入图像分辨率设置为1280×1024，两点之间的最小距离设置为0.01米。在“R3LIVE-RT”配置中，我们将输入图像分辨率设置为320×256，两点之间的最小距离设置为0.10米。

 

 B 面向各种3D应用领域前进

        利用开发的软件工具，我们可以将重建的3D地图导出到虚幻引擎，以实现一系列3D应用程序。例如，在图14中，我们使用AirSim构建了汽车和无人机模拟器，在图15中，我们使用我们重建的地图开发了桌面PC和移动平台的视频游戏。有关我们演示的更多详细信息，我们建议读者观看我们在YouTube上的视频。

图13：图（a）显示了由R3 LIVE重建的RGB彩色3D点云。图（b）和（c）显示了我们重建的网格的线框图和表面。图（d）显示了纹理渲染后的网格。

图14：我们使用R3 LIVE重建的地图来构建AirSim中的汽车（图a）和无人机（图b）模拟器。绿色和蓝色边框中的图像是从airsim的API查询的深度和RGB图像。

图15：我们使用R3 LIVE构建的地图开发了移动平台（见（a））和台式PC（见（b））上的视频游戏。在（a）中，玩家控制角色探索香港大学校园。在（b）中，玩家在香港科技大学校园内与龙战斗，用橡皮球射击。

REFERENCES

[1] J. Levinson, J. Askeland, J. Becker, J. Dolson, D. Held, S. Kammel, J. Z. Kolter, D. Langer, O. Pink, V. Pratt, et al., "Towards fully autonomous driving: Systems and algorithms," in 2011 IEEE Intelligent Vehicles Symposium (IV). IEEE, 2011, pp. 163-168.

[2] A. Bry, A. Bachrach, and N. Roy, "State estimation for aggressive flight in gps-denied environments using onboard sensing," in 2012 IEEE International Conference on Robotics and Automation. IEEE, 2012, pp. 1-8.

[3] F. Gao, W. Wu, W. Gao, and S. Shen, "Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments," Journal of Field Robotics, vol. 36, no. 4, pp. 710-733, 2019.

[4] F. Kong, W. Xu, Y. Cai, and F. Zhang, "Avoiding dynamic small obstacles with onboard sensing and computation on aerial robots," IEEE Robotics and Automation Letters, vol. 6, no. 4, pp. 7869-7876, 2021.

[5] Z. Liu, F. Zhang, and X. Hong, "Low-cost retina-like robotic lidars based on incommensurable scanning," IEEE Transactions on Mechatronics, 2021 , in press.

[6] 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, 2021, in press.

[7] J. Lin, X. Liu, and F. Zhang, "A decentralized framework for simultaneous calibration, localization and mapping with multiple lidars," in 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2020, pp. 4870-4877.

[8] Z. Liu and F. Zhang, "Balm: Bundle adjustment for lidar mapping," IEEE Robotics and Automation Letters, 2021, in press.
[9] X. Liu and F. Zhang, "Extrinsic calibration of multiple lidars of small fov in targetless environments," IEEE Robotics and Automation Letters, 2021, in press.

[10] C. Yuan, X. Liu, X. Hong, and F. Zhang, "Pixel-level extrinsic self calibration of high resolution lidar and camera in targetless environments," arXiv preprint arXiv:2103.01627, 2021.

[11] 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

[12] J. Lin, C. Zheng, W. Xu, and F. Zhang, " R2

 live: 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.

[13] T. Shan, B. Englot, C. Ratti, and D. Rus, "Lvi-sam: Tightly-coupled lidar-visual-inertial odometry via smoothing and mapping," arXiv preprint arXiv:2104.10831, 2021.

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

[15] X. Zuo, Y. Yang, J. Lv, Y. Liu, G. Huang, and M. Pollefeys, "Lic-fusion 2.0: Lidar-inertial-camera odometry with sliding-window plane-feature tracking," in IROS 2020, 2020.

[16] W. Zhen and S. Scherer, "Estimating the localizability in tunnel-like environments using lidar and uwb," in 2019 International Conference on Robotics and Automation (ICRA). IEEE, 2019, pp. 4903-4908.

[17] H. Zhou, Z. Yao, and M. Lu, "Uwb/lidar coordinate matching method with anti-degeneration capability," IEEE Sensors Journal, vol. 21, no. 3, pp. 3344-3352, 2020.

[18] C. Debeunne and D. Vivet, "A review of visual-lidar fusion based simultaneous localization and mapping," Sensors, vol. 20, no. 7, p. 2068 2020.

[19] 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.

[20] W. Shao, S. Vijayarangan, C. Li, and G. Kantor, "Stereo visual inertial lidar simultaneous localization and mapping," arXiv preprint arXiv:1902.10741, 2019.

[21] 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

[22] W. Xu, Y. Cai, D. He, J. Lin, and F. Zhang, "Fast-lio2: Fast direct lidar-inertial odometry," arXiv preprint arXiv:2107.06829, 2021.

[23] T. Qin and S. Shen, "Online temporal calibration for monocular visualinertial systems," in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 3662-3669.

[24] B. M. Bell and F. W. Cathey, "The iterated kalman filter update as a gauss-newton method," IEEE Transactions on Automatic Control, vol. 38, no. 2, pp. 294-297, 1993.

[25] S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez, "Automatic generation and detection of highly reliable fiducial markers under occlusion," Pattern Recognition, vol. 47, no. 6, pp. 2280-2292, 2014.

[26] 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.

[27] Z. Zhang and D. Scaramuzza, "A tutorial on quantitative trajectory evaluation for visual (-inertial) odometry," in 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2018, pp. 7244-7251.

[28] P. Labatut, J.-P. Pons, and R. Keriven, "Efficient multi-view reconstruction of large-scale scenes using interest points, delaunay triangulation and graph cuts," in 2007 IEEE 11th international conference on computer vision. IEEE, 2007, pp. 1-8.

[29] A. Fabri and S. Pion, "Cgal: The computational geometry algorithms library," in Proceedings of the 17th ACM SIGSPATIAL international conference on advances in geographic information systems, 2009, pp. 538-539.

[30] D. Girardeau-Montaut, "Cloudcompare," France: EDF R&D Telecom ParisTech, 2016.

[31] P. Cignoni, G. Ranzuglia, M. Callieri, M. Corsini, F. Ganovelli, N. Pietroni, and M. Tarini, "Meshlab," 2011.

[32] S. Shah, D. Dey, C. Lovett, and A. Kapoor, "Airsim: High-fidelity visual and physical simulation for autonomous vehicles," in Field and service robotics. Springer, 2018, pp. 621-635.