R3LIVE：一个实时鲁棒、带有RGB颜色信息的激光雷达-惯性-视觉紧耦合系统（1）

R3LIVE: A Robust, Real-time, RGB-colored, LiDAR-Inertial-Visual tightly-coupled state Estimation and mapping package

作者：Jiarong Lin and Fu Zhang（香港大学)

论文、代码地址：在公众号「计算机视觉工坊」，后台回复「R3LIVE」，即可直接下载。

视频：https://www.bilibili.com/video/BV1d341117d6?p=1&share_medium=android&share_plat=android&share_session_id=34022f35-58d8-49df-9aca-79b5cba2074d&share_source=WEIXIN&share_tag=s_i&timestamp=1631455968&unique_k=pmkmxs

https://www.bilibili.com/video/BV1e3411q7Di?p=1&share_medium=android&share_plat=android&share_session_id=f42a7bfc-8e53-488e-87ce-485d1cdffb40&share_source=WEIXIN&share_tag=s_i&timestamp=1631455997&unique_k=KIOPb0

摘要： 本文中，我们提出了一种称为 R3LIVE 的新型 LiDAR-Inertial-Visual 传感器融合框架，它利用 LiDAR、惯性和视觉传感器的测量来实现鲁棒和准确的状态估计。R3LIVE 包含两个子系统，即激光雷达-惯性里程计 (LIO) 和视觉-惯性里程计 (VIO)。LIO 子系统 (FAST-LIO) 利用 LiDAR 和惯性传感器的测量结果构建全局地图（即 3D 点的位置）的几何结构。VIO 子系统利用视觉-惯性传感器的数据来渲染地图的纹理（即 3D 点的颜色）。更具体地说，VIO 子系统通过最小化帧到地图的光度误差来直接有效地融合视觉数据。开发的系统 R3LIVE 是在我们之前的工作 R2LIVE 的基础上开发的，经过精心的架构设计和实现。实验结果表明，所得到的系统在状态估计方面比现有系统具有更强的鲁棒性和更高的精度。

R3LIVE 是一个面向各种可能应用的多功能且精心设计的系统，它不仅可以作为实时机器人应用的 SLAM 系统，还可以为测绘等应用重建密集、精确的 RGB 彩色 3D 地图 。此外，为了使 R3LIVE 更具可扩展性，我们开发了一系列用于重建和纹理化网格的离线实用程序，这进一步缩小了 R3LIVE 与各种 3D 应用程序（如模拟器、视频游戏等）之间的差距。

I 引言

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

Zhang and Singh提出的 V-LOAM [19] 是 LiDAR-Inertial-Visual 系统的早期作品之一，它利用松散耦合的视觉-惯性测距 (VIO) 作为初始化 LiDAR 映射子系统的运动模型。类似地，在 [20] 中，作者提出了一种立体视觉惯性 LiDAR SLAM，它结合了紧耦合的立体视觉-惯性里程计与 LiDAR 建图和 LiDAR 增强的视觉闭环。最近，Wang 提出了 DV-LOAM [21]，这是一个直接的 Visual-LiDAR 融合框架。该系统首先利用两阶段直接视觉里程计模块进行有效的粗略状态估计，然后使用 LiDAR 建图模块细化粗略姿态，最后利用闭环模块来校正累积漂移。上述系统在松耦合的水平上融合了 LiDAR 惯性视觉传感器，其中 LiDAR 测量没有与视觉或惯性测量一起联合优化。

最近提出了紧耦合的 LiDAR-Inertial-Visual 融合框架。例如，Zuo 等提出的 LIC-fusion [14] 是一个紧耦合的 LiDARInertial-Visual 融合框架，它结合了 IMU 测量、稀疏视觉特征、LiDAR 特征以及多状态约束卡尔曼滤波器内的在线空间和时间校准（ MSCKF) 框架。为了进一步增强 LiDAR 扫描匹配的鲁棒性，他们的后续工作称为 LIC-Fusion 2.0 [15] 提出了一种跨滑动窗口内多个 LiDAR 扫描的平面特征跟踪算法，并细化窗口内的姿态轨迹。Shan 等在 [13] 中提出 LVI-SAM 通过紧密耦合的平滑和建图框架融合 LiDAR-Visual-Inertial 传感器，该框架构建在因子图之上。LVI_SAM 的 LiDAR-Inertial 和 Visual-Inertial 子系统可以在其中之一检测到故障时独立运行，或者在检测到足够多的特征时联合运行。我们之前的工作 R2LIVE [12] 将 LiDAR-Inertial-Visual 传感器的数据紧密融合，提取 LiDAR 和稀疏视觉特征，通过在误差状态迭代卡尔曼滤波器框架内最小化特征重投影误差来估计状态，以实现实时性能，同时通过滑动窗口优化提高整体视觉映射精度。R2LIVE 能够在具有剧烈运动、传感器故障的各种具有挑战性的场景中运行，甚至可以在具有大量移动物体和小型 LiDAR FoV 的狭窄隧道状环境中运行。

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

我们提出了一个实时同步定位、建图和着色框架。所提出的框架包括用于重建几何结构的 LiDAR 惯性里程计 (LIO) 和用于纹理渲染的视觉惯性里程计 (VIO)。整个系统能够实时重建环境的稠密 3D RGB 色点云（图 1（a）），

我们提出了一种基于 RGB_colored 点云图的新型 VIO 系统。VIO 通过最小化观察到的地图点的 RGB 颜色与其在当前图像中的测量颜色之间的光度误差来估计当前状态。这样的过程不需要环境中的显着视觉特征并节省相应的处理时间（例如特征检测和提取），这使得我们提出的系统更加健壮，尤其是在无纹理环境中。

我们将所提出的方法实施到一个完整的系统 R3LIVE 中，该系统能够实时且低漂移地构建环境的稠密、精确、3D、RGB 彩色点云图。整个系统已在各种室内和室外环境中得到验证。结果表明，我们的系统在行驶 1.5 公里后，平移仅漂移 0.16 米，旋转漂移仅 3.9 度。

我们在 Github 上开源我们的系统。我们还开发了几种离线工具，用于从彩色点云重建和纹理化网格（见图 1（b）和（c））。我们设备的这些软件实用程序和机械设计也是开源的，以使可能的应用程序受益。

II 系统框架

我们系统的概述如图 2 所示，我们提出的框架包含两个子系统：LIO 子系统（上部）和 VIO 子系统（下部）。LIO 子系统构建了全局地图的几何结构，它记录了输入的 LiDAR 扫描，并通过最小化点到平面的残差来估计系统的状态。VIO 子系统构建贴图的纹理，用输入图像渲染每个点的 RGB 颜色，通过最小化帧到帧 PnP 重投影误差和帧到贴图光度误差来更新系统状态。

III. 数学符号

在整篇论文中，我们使用表 I 中所示的符号，这些符号已在之前的工作 R2LIVE [12] 中引入。

A．state

在我们的工作中，我们将完整状态向量 xϵ ℝ29 定义为：

其中 Gg ϵ ℝ3是在全局帧（即第一个 LiDAR 帧）中表示的重力矢量，ItC是 IMU 和相机之间的时间偏移，同时假设 LiDAR 已经与 IMU 同步，φ 是相机内参矩阵。

B. Maps representation

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

1) 体素：为了在我们的 VIO 子系统中快速找到地图中的点以进行渲染和跟踪（参见 Section.V-C 和 Section.V-D），我们设计了一个固定大小（例如 0.1m *0.1m *0.1m） 名为体素的容器。如果一个体素最近附加了点（例如最近 1 秒），我们将这个体素标记为已激活。否则，该体素被标记为停用。

2）point：在我们的工作中，点P是一个大小为6的向量（坐标和颜色RGB）

IV. 激光-惯性里程计子系统

如图 2 所示，R3LIVE 的 LIO 子系统构建了全局地图的几何结构。对于传入的 LiDAR 扫描，由于帧内连续移动而导致的运动失真由 IMU 反向传播补偿，如 [6] 所示。然后，我们利用误差状态迭代卡尔曼滤波器 (ESIKF) 最小化点对平面残差来估计系统的状态。最后，在收敛状态下，该扫描的点被附加到全局地图上，并将相应的体素标记为激活或停用。全局地图中累积的 3D 点形成几何结构，也用于为我们的 VIO 子系统提供深度。R3LIVE中LIO子系统的详细实现，请读者参考我们之前的相关工作[12, 22]。

V．视觉-惯性里程计子系统

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

A. Frame-to-frame Visual-Inertial odometry

残差(4)中的测量噪声有两个来源：一是中的像素跟踪误差，二是地图点位置误差

(12)中第一项的详细推导可以在R2LIVE [12]的E节中找到。