# LVI-SAM：基于平滑与建图的紧耦合激光雷达-视觉-惯性里程计 单铁超、恩格洛特·布伦丹、拉蒂·卡洛和鲁斯·丹妮拉 摘要—我们提出了一种基于平滑与建图的紧耦合激光雷达-视觉-惯性里程计框架LVI-SAM，该框架能够实现高精度且鲁棒性强的实时状态估计与地图构建。LVI-SAM基于因子图构建，由两个子系统组成：视觉-惯性系统（VIS）和激光雷达-惯性系统（LIS）。这两个子系统采用紧耦合方式设计，其中VIS利用LIS的估计结果来辅助初始化。通过利用激光雷达测量数据提取视觉特征的深度信息，VIS的精度得以提升。反过来，LIS则利用VIS的估计值作为初始猜测，以支持扫描匹配。VIS首先识别回环，随后由LIS进一步细化。即便其中一个子系统出现故障，LVI-SAM仍能正常运行，这提升了其在无纹理和无特征环境中的鲁棒性。LVI-SAM在多种平台采集的不同规模和环境数据集上经过了广泛测试。我们的实现代码可在获取。 ## 一、引言 同时定位与地图构建（SLAM）是许多移动机器人导航任务所需的基础能力。过去二十年间，借助激光雷达或摄像头等单一感知传感器，SLAM在复杂环境下的实时状态估计与地图构建方面取得了巨大成功。基于激光雷达的方法能够远距离捕捉环境的细微细节，但这类方法在无结构环境中（如长走廊或平坦开阔场地）往往无法正常工作。虽然基于视觉的方法尤其适用于位置识别，且在纹理丰富的环境中表现优异，但其性能易受光照变化、快速运动及初始化问题的影响。因此，基于激光雷达和基于视觉的方法通常都会与惯性测量单元（IMU）结合使用，以提升各自的鲁棒性和精度。激光雷达-惯性系统有助于校正点云畸变，并弥补短时间内的特征缺失问题；IMU测量数据可恢复度量尺度和姿态信息，从而辅助视觉-惯性系统。为了进一步提升系统性能，激光雷达、摄像头和IMU测量数据的融合正受到越来越多的关注。 我们的工作与视觉惯性里程计（VIO）、激光雷达惯性里程计（LIO）以及激光雷达-视觉惯性里程计（LVIO）最为相关。需要指出的是，本文并未考虑非惯性系统，尽管我们知晓存在一些成功的非惯性激光雷达-视觉系统，例如 [^1]、[^2]。视觉惯性里程计（VIO）可分为两大类：基于滤波的方法与基于优化的方法。基于滤波的方法通常利用扩展卡尔曼滤波（EKF），借助相机和IMU的测量数据来递推系统状态。基于优化的方法则采用滑动窗口估计器，同时最小化视觉重投影误差与IMU测量误差。在本研究中，我们仅考虑单目相机。在现有的主流开源VIO算法中，MSCKF [^3]、ROVIO [^4] 和 OpenVINS [^5] 属于基于滤波的方法，而OKVIS [^6] 和 VINS-Mono [^7] 则属于基于优化的方法。虽然OKVIS在使用双目相机时表现更优，但它并未针对单目相机进行优化。VINS-Mono在滑动窗口模式下执行非线性优化，凭借单目相机即可达到业界领先的精度 [^8]。 根据其设计方案，激光雷达惯性里程计也可分为两大类：松耦合方法与紧耦合方法。LOAM [^9] 和 LeGOLOAM [^10] 属于松耦合系统，因为优化步骤中并未使用IMU测量数据。紧耦合系统通常能提供更高的精度和鲁棒性，目前是相关研究的主要方向 [^11]。在现有的开源紧耦合系统中，LIO-mapping [^12] 采用了 [^7] 的优化流程，旨在最小化IMU与激光雷达测量的残差。由于LIO-mapping旨在优化所有测量数据，因此无法达到实时性能。LIO-SAM [^13] 通过引入激光雷达关键帧的滑动窗口来限制计算复杂度，利用因子图进行IMU与激光雷达约束的联合优化。专为地面车辆设计的LINS [^14] 则采用误差状态卡尔曼滤波器来递归修正机器人的状态。 近年来，由于在传感器性能下降的任务中表现出色，激光雷达-视觉-惯性系统受到了越来越多的关注 $^{[15]}$。[^16] 提出了一种紧耦合的LVIO系统，该系统采用顺序处理流程，从粗略到精细地解决状态估计问题。粗略估计始于IMU预测，随后通过VIO和LIO进一步细化。[^16] 目前在KITTI基准测试中取得了业界领先的精度 $^{[17]}$。基于MSCKF框架，[^18] 具备在线时空多传感器校准功能。[^16] 和 [^18] 的实现代码并未公开。我们的工作与上述研究有所不同，因为我们利用因子图进行全局优化，通过回环闭合检测定期消除机器人产生的漂移误差。 --- ![Image](images/a336fa1cc18ff1ffedc6c7acbfc32d4e8a4093dc03ec96ae0e39b87d9a63769e.jpg)
Fig. 1: The system structure of LVI-SAM. The system, which receives input from a 3D lidar, a camera and an IMU, can be divided into two sub-systems: a visual-inertial system (VIS) and a lidar-inertial system (LIS). The VIS and LIS can function independently while using information from each other to increase system accuracy and robustness. The system outputs pose estimates at the IMU rate.
在本文中，我们提出了一种基于平滑与建图的紧耦合激光雷达-视觉-惯性里程计框架LVI-SAM，用于实时状态估计与地图构建。LVI-SAM以因子图为基础，由两个子系统组成：视觉-惯性系统（VIS）和激光雷达-惯性系统（LIS）。当其中一个子系统发生故障时，这两个子系统可以独立运行；一旦检测到足够多的特征点，它们便可协同工作。VIS负责视觉特征跟踪，并可选择性地利用激光雷达帧提取特征深度。通过优化视觉重投影误差与IMU测量值得到的视觉里程计结果，可作为激光雷达扫描匹配的初始估计，并为因子图引入约束条件。利用IMU测量值完成点云去畸变处理后，LIS会提取激光雷达的边缘与平面特征，并将其与存储在滑动窗口中的特征地图进行匹配。LIS中估算出的系统状态可被发送至VIS，以辅助其初始化。对于回环闭合，首先由VIS识别出候选匹配项，随后再由LIS对其进行优化。视觉里程计、激光雷达里程计、IMU预积分以及回环闭合所产生的约束条件会在因子图中被联合优化。最后，优化后的IMU偏差项会被用来按IMU采样率传播IMU测量值，从而实现姿态估计。本研究的主要贡献可总结如下： - 一种基于因子图构建的紧耦合LVIO框架，它不仅能实现多传感器融合，还能借助地点识别技术完成全局优化。 - 我们的框架可通过故障检测机制绕过失效的子系统，从而具备应对传感器性能退化的鲁棒性。 - 我们的框架已在不同尺度、平台和环境下收集的数据上经过了充分验证。从系统层面来看，我们的工作颇具创新性，它将最先进的VIO与LIO技术独特地整合在一起，打造出鲁棒性与精度均有所提升的LVIO系统。我们希望该系统能成为一个可靠的基准，供他人在此基础上进一步拓展，推动激光雷达-视觉-惯性里程计领域的技术进步。 ### II. 基于平滑与建图的激光雷达视觉惯性里程计 ### A. 系统概述 图1展示了所提出的激光雷达-视觉-惯性系统的概览，该系统接收来自3D激光雷达、单目相机以及IMU的输入。 ![Image](images/31f742dbc48944f779f3f44bf7a3f56ce82ab10bd09c7f87246b4522fca07337.jpg)
Fig. 2: The framework of our visual-inertial system. The system optimizes residuals from IMU preintegration, visual measurements without depth, and visual measurements with depth.
我们的框架由两个关键子系统构成：视觉-惯性系统（VIS）和激光雷达-惯性系统（LIS）。VIS负责处理图像与IMU测量数据，激光雷达测量数据则为可选输入。视觉里程计是通过最小化视觉与IMU测量值的联合残差得到的。LIS则负责提取激光雷达特征，并通过将提取的特征与特征地图匹配来完成激光雷达里程计计算。为了兼顾实时性能，特征地图采用滑动窗口方式维护。最后，状态估计问题可表述为最大后验概率（MAP）问题，该问题通过利用iSAM2 $^{[19]}$在因子图中联合优化IMU预积分约束、视觉里程计约束、激光雷达里程计约束以及回环闭合约束来求解。需要注意的是，LIS中所采用的多传感器图优化技术旨在减少数据交换并提升系统效率。 ### B. 视觉-惯性系统 我们采用了来自[^7]的VIS处理流程，如图2所示。视觉特征通过角点检测器[^20]提取，随后由Kanade–Lucas–Tomasi算法[^21]进行跟踪。在VIS初始化阶段，我们利用视觉里程计对激光雷达帧进行配准，从而获得用于特征深度估计的稀疏深度图像。该系统在滑动窗口模式下执行束调整，此时系统状态$\mathbf{x} \in \mathbf{X}$可表示为： $$ \mathbf{x}=[\mathbf{R},\mathbf{p},\mathbf{v},\mathbf{b}]. $$ $\mathbf{R} \in SO(3)$为旋转矩阵，$\mathbf{p} \in \mathbb{R}^3$为位置矢量，$\mathbf{v}$表示速度，而$\mathbf{b} = [\mathbf{b}_a, \mathbf{b}_w]$则为惯性测量单元（IMU）的偏差。 --- $b_a$和$b_w$分别是加速度与角速度的偏差向量。从传感器机体坐标系$\mathbf{T} \in SE(3)$到世界坐标系$\mathbf{B}$的变换$\mathbf{W}$可表示为$\mathbf{T} = [\mathbf{R}|\mathbf{p}]$。在后续章节中，我们将详细介绍优化VIS初始化与特征深度估计的具体步骤。由于篇幅有限，更多细节（如残差项的实现方式）请参阅[^7]。 1) 初始化：基于优化的视觉惯性观测（VIO）在初始化阶段往往因求解高度非线性问题而出现发散现象。初始化的质量主要取决于两个因素：传感器的初始运动情况以及IMU参数的准确性。实践中我们发现，当传感器以低速或恒定速度运动时，[^7]常常无法完成初始化。这是因为当加速度激励不足时，无法观测到度量尺度。IMU参数包含缓慢变化的偏差和白噪声，它们会影响原始的加速度与角速度测量结果。初始化时对这些参数做出合理预估，有助于优化过程更快收敛。 为了提升VIS初始化的鲁棒性，我们利用了激光惯性系统（LIS）估算出的系统状态x和IMU偏差b。由于激光雷达可直接获取深度信息，我们首先通过LIS完成初始化并得到x和b。随后根据图像时间戳将它们插值并关联到各个图像关键帧上；需注意，我们假设两个图像关键帧之间的IMU偏差保持恒定。最终，将LIS估算出的x和b作为VIS初始化的初始值，这显著提高了初始化的速度与鲁棒性。有关有无LIS辅助的VIS初始化对比，可观看补充视频$^{1}$。 2) 特征深度关联：VIS初始化完成后，我们利用估算出的视觉里程计数据将激光雷达帧配准到相机坐标系下。由于现代三维激光雷达通常只能生成稀疏扫描数据，因此我们需要堆叠多帧激光雷达数据以获得密集的深度图。为了将特征与深度值对应起来，我们首先将视觉特征与激光雷达深度点投影到以相机为中心的单位球面上。随后对深度点进行下采样，并利用极坐标存储以保证球面上的密度均匀。我们通过二维K-D树，依据视觉特征的极坐标在球面上查找距离最近的三个深度点。最后，特征深度即为视觉特征与相机中心连线$O_{c}$在笛卡尔空间中与三个深度点所在平面相交所形成的线段长度。该过程的可视化效果见图3(a)，其中特征深度即为虚线的长度。 我们进一步检查相关特征深度的有效性，方法是计算三个最近深度点之间的距离。这是因为来自不同时间戳的激光雷达帧叠加可能会导致不同物体产生的深度信息出现歧义。图3(b)展示了这种情况的示例。在$t_{i}$时刻观测到的深度点以绿色标示。当摄像头移动到$t_{j}$时刻的新位置时，会观测到新的深度信息。 ![Image](images/e1a251518e112c6a0898897736ecaebc788d7f8d9123dc25e53c092ff64ef4cc.jpg)
(a) Depth association
![Image](images/2fdefd962c9df02a8d3b067f985b48fdb3e22562bd72d3e17a9f8bf948868c31.jpg)
(b) Depth validation
Fig. 3: Visual feature depth association.
![Image](images/8bd0b6cac1515610b9080dc40f1e9e06d141e51ac28bdebe2b0f56852a613a9a.jpg)
(a)
![Image](images/ec85d7f4083def9009f5cb8fae5099326ba2c5b87c39d0dddc18384f38b4fa9d.jpg)
(b)
![Image](images/84e2588144ec2922eed1bdbf96097e2341cf27606c5d08ad5367afb542692fa7.jpg)
(c)
![Image](images/d7546715e4e82b807cb1ee59701ca5e85df25fe665e1523a35ae1863265b6e6f.jpg)
(d)
Fig. 4: Registered depth map and visual features. In (a) and (c), the color variation of the depth map indicates depth change. In (b) and (d), green points are visual features that are successfully associated with depth. Features that fail the depth association process are red.
这些点被标记为灰色。然而，在$t_i$时刻出现的、由虚线灰色圆圈标出的深度点，由于激光雷达帧叠加的原因，在$t_j$时刻仍可能被观测到。利用来自不同物体的深度点来关联特征深度会导致估算结果不准确。与[^16]类似，我们通过检查某一特征对应的深度点之间的最大距离来排除这类不准确的估算结果。如果最大距离超过2米，则说明该特征没有对应的深度信息。 图4展示了配准后的深度图与视觉特征的示例。在图4(a)和(c)中，通过视觉里程计配准的深度点被投影到摄像头图像上。在图4(b)和(d)中，成功与深度信息关联起来的视觉特征显示为绿色。需要注意的是，尽管图4(a)中的深度图覆盖了图像的大部分区域，但由于验证检查未通过，图4(b)中位于窗户角落处的许多特征仍没有对应的深度信息。 3) 故障检测：由于剧烈运动、光照变化以及无纹理环境的影响，VIS可能会出现故障。当机器人进行剧烈运动或进入无纹理环境时，被追踪的特征数量会大幅减少。特征数量不足可能导致优化过程发散。我们还发现，当VIS发生故障时，估算出的IMU偏差会很大。因此，当被追踪的特征数量低于阈值时，我们便判定VIS发生故障。 --- ![Image](images/42f5302029a23d0da7deab62801690d338082f8d092847fd92d59fb02ae10299.jpg)
Fig. 5: The framework of our lidar-inertial system. The system maintains a factor graph that has four types of constraints.
或者当估算出的IMU偏差超过阈值时，也判定为VIS故障。我们的系统需要主动进行故障检测，以免故障影响LIS的正常功能。一旦检测到故障，VIS会重新初始化并向LIS发送通知。 4) 回环检测：我们利用DBoW2 [^22]来进行回环检测。对于每个新的图像关键帧，我们提取BRIEF描述符 [^23] 并将其与之前提取的描述符进行匹配。DBoW2返回的回环候选图像的时间戳会被发送给LIS以作进一步验证。 ### C. 激光雷达-惯性系统 如图5所示，所提出的激光雷达-惯性系统借鉴了[^13]中的设计，该系统会维护一个用于全局位姿优化的因子图。图中加入了四种约束：IMU预积分约束、视觉里程计约束、激光雷达里程计约束以及回环闭合约束，并对其进行联合优化。激光雷达里程计约束源自扫描匹配过程，即将当前的激光雷达关键帧与全局特征地图进行匹配。回环闭合约束的候选结果首先由VIS提供，随后再通过扫描匹配进一步优化。我们为特征地图维护一个滑动窗口式的激光雷达关键帧集合，以此确保计算复杂度保持在可控范围内。当机器人位姿的变化超过阈值时，便会选取新的激光雷达关键帧；而位于相邻关键帧之间的零散激光雷达帧则会被丢弃。一旦选定新的激光雷达关键帧，就会将对应的新机器人状态x作为节点添加到因子图中。以这种方式添加关键帧，不仅能在内存消耗与地图密度之间取得平衡，还有助于维持相对稀疏的因子图以实现实时优化。由于篇幅有限，关于实现细节请参阅[^13]。在后续章节中，我们将重点介绍那些能够提升系统鲁棒性的新流程。 1) 初始猜测：我们发现初始猜测对扫描匹配的成功至关重要，尤其是在传感器经历剧烈运动的情况下。在LIS初始化前后，初始猜测的来源有所不同。 在LIS初始化之前，我们假设机器人从静止位置出发，初始速度为零。随后我们基于零偏差和零噪声的假设来积分原始的IMU测量数据。通过两个激光雷达关键帧之间的平移与旋转变化量，便可得到扫描匹配所需的初始猜测。我们发现这种方法能够在初始线速度低于10 m/s且角速度低于180 °/s的复杂条件下成功完成系统初始化。一旦LIS初始化完成，系统便会在因子图中估算出IMU偏差、机器人位姿以及速度信息，随后将这些数据发送给VIS以帮助其完成初始化。 ![Image](images/3dd7d0b5ebe9f18cdf69d34c11958981c352e916173990a6839e5f376c787adb.jpg) ![Image](images/ab2080eac33f7339ca0eb1cdf7b5c1eec42c53966664a38296d033471a6de4e3.jpg)
(a)
![Image](images/3440ed9a8302014c85f2cb4c90a07f1ef3d46a14f45acedbbc787930982d26d7.jpg)
(b)
(c)
![Image](images/50f245b5296cd56024e68acb87b46a78ee751beccce5bfabcf2ad89fc8109c1e.jpg)
(d)
Fig. 6: Degenerate mapping scenarios where scan-matching is ill-constrained. In (a) and (c), the lidar is placed facing the ground. In (b) and (d), the lidar is in a flat, open, and structure-less environment. White points indicate the contents of the lidar scan. Color variation indicates elevation change.
该系统能够在初始线速度小于10 m/s、角速度小于180 °/s的复杂环境下正常工作。一旦LIS完成初始化，我们便会在因子图中估算IMU偏差、机器人位姿及速度，随后将其发送给VIS以辅助其初始化。 LIS初始化之后，我们可以从两个来源获取初始猜测：经过偏差校正后的IMU测量数据，以及VIS。当VIS提供的初始猜测可用时，我们便将其用作初始猜测；若VIS无法提供有效结果，则会切换为使用IMU测量数据作为初始猜测。这些流程无论是在纹理丰富还是纹理匮乏的环境中，都能有效提升初始猜测的精度与鲁棒性。 2) 故障检测：尽管激光雷达能够在远距离下捕捉环境的精细细节，但在某些场景下其性能仍会下降，导致扫描匹配受到严重限制。此类场景如图6所示。我们采用了[^24]中提出的方法来进行LIS故障检测。扫描匹配中的非线性优化问题可转化为迭代求解线性问题的过程： $$ \min_{\mathbf{T}}\|\mathbf{A}\mathbf{T}-\mathbf{b}\|^{2}, $$ 其中$\mathbf{A}$和$\mathbf{b}$是在$\mathbf{T}$处线性化后得到的数值。若在优化的第一轮迭代中，$\mathbf{A}^{\top}\mathbf{A}$的最小特征值小于设定阈值，LIS便会判定发生故障。一旦发生故障，激光雷达里程计约束便不会被添加到因子图中。关于这些假设背后的详细分析，请参阅[^24]。 ### 三、实验 接下来我们将描述一系列实验，旨在利用三个自行采集的数据集来验证所提出的框架。 --- ![Image](images/aefe6f572ad09ec45195ed4d8a8fff873ac3527341507848043a63d0a04779c4.jpg)
Fig. 7: Trajectories of ablation study using Urban dataset.
TABLE I: End-to-end translation and rotation errors.
Error typeA1 (w/o depth)A1 (w/ depth)A2A3 (w/o depth)A3 (w/ depth)A4
Translation (m)239.19142.12290.4345.4232.180.28
Rotation (degree)60.7239.52116.506.417.665.77
这些数据集分别被称为Urban、Jackal和Handheld。各数据集的详细信息将在后续章节中介绍。我们用于数据采集的传感器套件包括Velodyne VLP-16激光雷达、FLIR BFS-U3-04S2M-CS相机、MicroStrain 3DM-GX5-25惯性测量单元以及Reach RS+ GPS（用于地面真实值验证）。我们将所提出的框架与开源解决方案进行对比，这些方案包括VINS-Mono、LOAM、LIO-mapping、LINS和LIO-SAM。所有方法均用C++实现，并在搭载Intel i7-10710U处理器的笔记本电脑上的Ubuntu Linux系统中运行。我们的LVI-SAM实现及数据集可通过下方链接获取 $^{2}$。 ### A. 消融实验 我们利用Urban数据集来展示系统中各个模块的设计对所提出框架性能的影响。该数据集包含建筑物、停放及行驶中的汽车、行人、自行车和植被等元素，是由操作员携带传感器套件步行采集而成的。我们还特意将传感器套件放置在具有挑战性的位置（图6(a)），以验证系统在恶劣环境下的鲁棒性。由于上方植被茂密，该区域属于GPS信号盲区。数据采集过程从同一位置开始并结束，以便评估端到端的平移与旋转误差，相关结果见表I。 1) A1——在视觉惯性里程计中加入激光雷达特征深度信息的影响：我们禁用LIS中的扫描匹配功能，仅依靠VIS进行位姿估计。启用与未启用深度配准时的轨迹分别在图中标记为A1。 ![Image](images/cd0c43c6e680f5216f2f34c2c86d6cb4ebe35451771f9f630cdc8c446c09ed96.jpg)
(a) Jackal dataset environment
![Image](images/6f7194a300abf061993acc609cfcd0ee8827785317b9e3b94437f270006fdedd.jpg)
(b) Handheld dataset environment
Fig. 8: Satellite images where the Jackal and Handheld datasets were gathered. White dots indicate GPS availability in the datasets.
图7中轨迹方向为顺时针。如表I所示，当将深度信息与视觉特征关联时，端到端位姿误差显著减小。 2) A2——加入视觉惯性里程计的影响：我们禁用VIS，仅使用LIS进行位姿估计。图7中标记为A2的轨迹在遇到恶劣环境时会出现几次偏离。 3) A3——在激光雷达-视觉-惯性里程计中加入激光雷达特征深度信息的影响：此时我们同时使用VIS和LIS，并通过切换VIS中的深度配准模块来对比所得的LVIO轨迹。借助视觉特征的深度信息，平移误差进一步降低了29%，从45.42米减小到32.18米。需注意，本次测试中禁用了回环闭合检测功能，旨在验证系统在纯里程计模式下的表现。 4) A4——加入视觉回环闭合检测的影响：我们通过启用VIS中的回环闭合检测功能来消除系统的漂移现象。框架中所有模块均启用时的最终轨迹在图7中标记为A4。 ### B. Jackal数据集 Jackal数据集是通过将传感器套件安装在Clearpath Jackal无人地面车辆（UGV）上采集而成的。我们在特征丰富的环境中手动驾驶该机器人，采集起点与终点均为同一位置。图8(a)展示了该环境，其中包含建筑物、植被以及各种路面。能够接收到GPS信号的区域用白点标记。 我们比较了各种方法，并在图9(a)中展示了它们的发展轨迹。我们通过手动禁用和启用具有闭环功能的算法，进一步验证了这些算法的准确性。基准测试结果如表II所示。LVI-SAM相对于作为真实值的GPS测量数据，其平均均方根误差(RMSE)最小。而LINS则实现了最低的端到端平移误差，该算法源自LeGOLOAM $^{[10]}$，专为无人地面车辆(UGV)操作而设计。最低的端到端旋转误差同样由LVI-SAM实现。 ### C. 手持式数据集 手持式数据集是由操作员携带传感器套件在多个开阔场地中行走时采集的， --- ![Image](images/9861b9666e0106d8cc53569f0859966e2f67c7db518c319f2f74e8d1ff368708.jpg) ![Image](images/dbe072112c962628fd53a1aaded6f8420d72fa8402dbd6e26a48f0505f7b2937.jpg) ![Image](images/33cb4764820a7b9c3454cbc0e7f01cf1eea03023e5cb683d241a107206b30637.jpg) ![Image](images/3d0ee5e933d94c6b0eb3e135d58adf09092fedca2ac3de2e44078e14e1075d16.jpg) ![Image](images/115685bb52a931a71e3fc886be4fcb3b877f1f3ff49d8f4ac9ab02448cdd16a7.jpg) ![Image](images/58959bd82f0262959aeafe13617862fc3f9261bee40990c21034a9730a54793a.jpg) ![Image](images/8903975fbf57a006fe65e6940cc5352bc27e83f114c0563f478defa1951477cc.jpg) ![Image](images/f5d2bb71ad34278bdcebe0db7b1ccd6dd1a0f3ba3ad5272bb4d372db4e250d3f.jpg) ![Image](images/5cb135c4a45ee981cf57c1d6501f32df3675ea514c51d2b18fcb9b5cacc1c861.jpg) ![Image](images/8859c0f300b97a0e0ffe035579e2f45bff113a145b72888bed63ea6643d38efc.jpg)
(a) Trajectories of Jackal dataset
![Image](images/af6e8b55b6e76188b82ef9e399688db3e4d98bdf3f91435ce98b792ab440965d.jpg) ![Image](images/7a47523ed3117ece1245435224825dbdb9b870a88c39da8a08a52a92d3b97ad6.jpg) ![Image](images/e124a73792049c5719d58150b46c968f777f0a9699c07a6e14d87e5253bbe6b0.jpg) ![Image](images/a1bffb400a71f4596e64237a2629cc8bd8225d239967abb4a8757fd4a32b9ce8.jpg)
(b) Trajectories of Handheld dataset
Fig. 9: Trajectories of various methods using the Jackal and Handheld datasets. The green line depicts the resulting trajectory from each compared method. The GPS positioning measurements, which serve as the ground truth, are shown with red dots. Trajectories of LOAM, LIO-mapping, LINS, and LIO-SAM are not shown due to their failure to generate meaningful results.
TABLE II: Quantitative comparison of Jackal and Handheld datasets using various methods.
DatasetError typeVINS (w/o loop)VINS (w/ loop)LOAMLIO-mappingLINS (w/o loop)LINS (w/ loop)LIO-SAM (w/o loop)LIO-SAM (w/ loop)LVI-SAM (w/o loop)LVI-SAM (w/ loop)
JackalRMSE w.r.t GPS (m)8.584.4944.92127.053.950.773.541.524.050.67
Translation (m)10.8211.8661.73123.227.370.095.480.124.690.11
Rotation (degree)9.9812.7961.41139.234.802.072.182.642.281.52
HandheldRMSE w.r.t GPS (m)87.5373.07FailFailFailFail53.62Fail7.870.83
Translation (m)40.651.87FailFailFailFail58.91Fail7.570.27
Rotation (degree)53.4852.09FailFailFailFail20.17Fail24.823.82
### IV. 结论 我们提出了LVI-SAM，这是一个通过平滑处理来实现激光雷达-视觉-惯性里程计紧密耦合的框架， 相关内容如图8(b)所示。该数据集的起点和终点位置相同。为了增加该数据集的难度，我们让设备穿过位于图像顶部中央的一片开放式棒球场。经过该区域时，摄像头和激光雷达分别采集到草地和地面的特征（见图6(b)和(d)）。由于上述的简并问题，所有基于激光雷达的方法都无法生成有意义的结果。而我们提出的LVI-SAM框架无论是否开启闭环功能，都能成功完成测试，且在表II所列的三项基准测试中均取得了最低误差。 该框架用于复杂环境中的实时状态估计与建图。该框架由两个子系统组成：视觉-惯性系统和激光雷达-惯性系统。这两个子系统以紧密耦合的方式协同工作，以提升系统的鲁棒性和精度。通过在不同规模、平台和环境下的数据集上进行评估，我们的系统展现出与现有公开方法相当甚至更优的精度。我们希望该系统能成为一个可靠的基准，供其他人在此基础上进一步拓展，推动激光雷达-视觉-惯性里程计领域的技术进步。 ## 致谢 本研究得到了荷兰阿姆斯特丹先进大都市解决方案研究所的支持。 --- ## 参考文献 [^1]: J. Graeter, A. Wilczynski, and M. Lauer, "LIMO: Lidar-Monocular Visual Odometry," IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 7872–7879, 2018. [^2]: 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. [^3]: A. I. Mourikis and S. I. Roumeliotis, “A Multi-state Constraint Kalman Filter for Vision-aided Inertial Navigation,” IEEE International Conference on Robotics and Automation (ICRA), pp. 3565–3572, 2007. [^4]: M. Bloesch, S. Omari, M. Hutter, and R. Siegwart, “Robust Visual Inertial Odometry using A Direct EKF-based Approach,” IEEE/RSJ international conference on intelligent robots and systems (IROS), pp. 298–304, 2015. [^5]: P. Geneva, K. Eckenhoff, W. Lee, Y. Yang, and G. Huang, “OpenVINS: A Research Platform for Visual-Inertial Estimation,” IROS Workshop on Visual-Inertial Navigation: Challenges and Applications, 2019. [^6]: S. Leutenegger, S. Lynen, M. Bosse, R. Siegwart, and P. Furgale, “Keyframe-based Visual-Inertial Odometry using Nonlinear Optimization,” The International Journal of Robotics Research, vol. 34, no. 3, pp. 314–334, 2015. [^7]: 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. [^8]: J. Delmerico and D. Scaramuzza, “A Benchmark Comparison of Monocular Visual-Inertial Odometry Algorithms for Flying Robots,” IEEE International Conference on Robotics and Automation (ICRA), pp. 2502–2509, 2018. [^9]: J. Zhang and S. Singh, "Low-drift and Real-time Lidar Odometry and Mapping," Autonomous Robots, vol. 41, no. 2, pp. 401–416, 2017. [^10]: T. Shan and B. Englot, “LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain,” IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4758–4765, 2018. [^11]: C. Chen, H. Zhu, M. Li, and S. You, “A Review of Visual-Inertial Simultaneous Localization and Mapping from Filtering-based and Optimization-based Perspectives,” Robotics, vol. 7, no. 3, p. 45, 2018. [^12]: H. Ye, Y. Chen, and M. Liu, “Tightly Coupled 3D Lidar Inertial Odometry and Mapping,” IEEE International Conference on Robotics and Automation (ICRA), pp. 3144–3150, 2019. [^13]: T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, "LIOSAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping," IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 4758–4765, 2020. [^14]: C. Qin, H. Ye, C. E. Pranata, J. Han, S. Zhang, and M. Liu, "LINS: A Lidar-Inertial State Estimator for Robust and Efficient Navigation," IEEE International Conference on Robotics and Automation (ICRA), pp. 8899–8906, 2020. [^15]: C. Debeunne and D. Vivet, “A Review of Visual-LiDAR Fusion based Simultaneous Localization and Mapping,” Sensors, vol. 20, no. 7, p. 2068, 2020. [^16]: 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. [^17]: A. Geiger, P. Lenz, C. Stiller, and R. Urtasun, “Vision Meets Robotics: The KITTI Dataset,” The International Journal of Robotics Research, vol. 32, no. 11, pp. 1231–1237, 2013. [^18]: X. Zuo, P. Geneva, W. Lee, Y. Liu, and G. Huang, "LIC-Fusion: LiDAR-Inertial-Camera Odometry," arXiv preprint arXiv:1909.04102, 2019. [^19]: M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. J. Leonard, and F. Dellaert, "iSAM2: Incremental Smoothing and Mapping using the Bayes Tree," The International Journal of Robotics Research, vol. 31, no. 2, pp. 216–235, 2012. [^20]: J. Shi et al., “Good Features to Track,” IEEE Conference on Computer Vision and Pattern Recognition, pp. 593–600, 1994. [^21]: B. D. Lucas, T. Kanade et al., “An Iterative Image Registration Technique with an Application to Stereo Vision,” 1981. [^22]: D. Gálvez-López and J. D. Tardos, “Bags of Binary Words for Fast Place Recognition in Image Sequences,” IEEE Transactions on Robotics, vol. 28, no. 5, pp. 1188–1197, 2012. [^23]: M. Calonder, V. Lepetit, C. Strecha, and P. Fua, “Brief: Binary robust independent elementary features,” European conference on computer vision, pp. 778–792, 2010. [^24]: J. Zhang, M. Kaess, and S. Singh, “On Degeneracy of Optimization-based State Estimation Problems,” IEEE International Conference on Robotics and Automation (ICRA), pp. 809–816, 2016.