Jiarong Lin and Fu Zhang(香港大学)
在公众号「」，后台回复「」，可直接下载。
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×tamp=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×tamp=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 应用程序(如模拟器、视频游戏等。)之间的差距。
最近，激光雷达传感器越来越多地用于各种机器人应用，如自动驾驶汽车 无人机 [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 在中间，该系统可以实时、低漂移地构建密集、准确的环境D、RGB 彩点云图。在各种室内外环境室内外环境中得到验证。结果表明，我们的系统正在行驶 1.5 公里后，平移只漂移 0.16 旋转漂移仅为米 3.9 度。
我们在 Github 上开源我们的系统。我们还开发了几种从彩色点云重建和纹理网格的离线工具 1（b）和（c））。这些软件能的应用程序受益，我们设备的软件实用程序和机械设计也是开源的。
我们系统的概述如图所示 2 我们提出的框架包括两个子系统：LIO 子系统(上部)和 VIO 子系统(下部)。LIO 子系统构建了全局地图的几何结构，记录了输入 LiDAR 扫描系统的状态，并通过最小化点到平面的残差来估计。VIO 子系统构建贴图纹理，用输入图像渲染每个点 RGB 颜色从帧最小化到帧 PnP 将系统状态更新为重投影误差和帧到贴图光度误差。
在整篇论文中，我们使用表格 I 这些符号已经在以前工作了 R2LIVE [12] 中引入。
在我们的工作中，我们将完全处于向量状态 x? ?29 定义为：
其中 Gg ? ?3是全局帧(即第一个) LiDAR 帧)中表示的重力矢量，ItC是 IMU 假设与相机之间的时间偏移 LiDAR 已经与 IMU 同步，φ 是相机内参矩阵。
我们的地图由体素和点组成，其中点包含在体素中并且是地图的最小元素。
1) 体素：为了在我们的 VIO 子系统中快速找到地图中的点以进行渲染和跟踪（参见 Section.V-C 和 Section.V-D），我们设计了一个固定大小（例如 0.1m *0.1m *0.1m） 名为体素的容器。如果一个体素最近附加了点（例如最近 1 秒），我们将这个体素标记为已激活。否则，该体素被标记为停用。
2）point：在我们的工作中，点P是一个大小为6的向量（坐标和颜色RGB）
如图 2 所示，R3LIVE 的 LIO 子系统构建了全局地图的几何结构。对于传入的 LiDAR 扫描，由于帧内连续移动而导致的运动失真由 IMU 反向传播补偿，如 [6] 所示。然后，我们利用误差状态迭代卡尔曼滤波器 (ESIKF) 最小化点对平面残差来估计系统的状态。最后，在收敛状态下，该扫描的点被附加到全局地图上，并将相应的体素标记为激活或停用。全局地图中累积的 3D 点形成几何结构，也用于为我们的 VIO 子系统提供深度。R3LIVE中LIO子系统的详细实现，请读者参考我们之前的相关工作[12, 22]。
我们的 VIO 子系统渲染全局贴图的纹理，通过最小化光度误差来估计系统状态。更具体地说，我们将全局地图中的一定数量的点（即跟踪点）投影到当前图像，然后通过最小化这些点的光度误差来迭代估计 ESIKF 框架内的系统状态。为了提高效率，跟踪的地图点是稀疏的，这通常需要构建输入图像的金字塔。然而，金字塔对于也需要估计的平移或旋转不是不变的。在我们提出的框架中，我们利用单个地图点的颜色来计算光度误差。在 VIO 中同时渲染的颜色是地图点的固有属性，并且不受相机平移和旋转的影响。为了确保稳健且快速的收敛，我们设计了如图 2 所示的两步框架，我们首先利用帧到帧光流来跟踪地图点并通过最小化 Perspective-n-Point (PnP)来优化系统状态跟踪地图点的投影误差（第 VA 部分）。然后，我们通过最小化跟踪点之间的帧到地图光度误差来进一步细化系统的状态估计（第 V-B 部分）。使用收敛状态估计和原始输入图像，我们执行纹理渲染以更新全局地图中点的颜色（第 V-C 部分）。
残差(4)中的测量噪声有两个来源：一是中的像素跟踪误差，二是地图点位置误差
(12)中第一项的详细推导可以在R2LIVE [12]的E节中找到。
我们还考虑了 γs 和 cs 的测量噪声：
结合(19)、(20)和(21)，我们得到真零残差的一阶泰勒展开式：
2）Frame-to-map VIO ESIKF更新：方程（22）构成了的另一个观测分布，它与来自IMU传播的先验分布相结合，得到的最大后验（MAP）估计：
然后，我们执行类似于（17）和（18）的状态更新。这个帧到地图 VIO ESIKF 更新（第 V-B1 部分到第 V-B2 部分）被迭代直到收敛。然后将收敛状态估计用于：(1) 渲染地图的纹理（第 V-C 部分）；(2) 更新当前跟踪点集 P 以供下一帧使用（Section V-D）；(3) 在 LIO 或 VIO 更新的下一帧中作为 IMU 传播的起点
在frame-to-map VIO更新之后，我们有了当前图像的精确位姿，然后我们执行渲染函数来更新地图点的颜色。
纹理渲染完成后，我们对跟踪点集 进行更新。不落入Ik。其次，我们将 ζ 中的每个点投影到当前图像 Ik，如果附近没有其他跟踪点（例如在 50 个像素的半径内），则将其添加到 。
the onboard DJI manifold-2c5 computation platform (equipped with an Intel i7-8550u CPU and 8 GB RAM), a FLIR Blackfly BFS-u3-13y3c global shutter camera, and a LiVOX AVIA6 LiDAR. The FoV of the camera is 82.9°*66.5°, while the FoV of the LiDAR is 70.4°* 77.2°.
如图 7 所示，我们的传感器穿过狭窄的“T”形通道，同时偶尔面对侧壁。当面对仅施加单个平面约束的墙壁时，众所周知，LiDAR 对于完整姿态估计会退化。同时，白色墙壁上的视觉纹理非常有限（图 7（a）和图 7（c）），尤其是墙壁，它只有光照变化。这种场景对于基于 LiDAR 和基于视觉的 SLAM 方法都具有挑战性。
图 8 显示了我们估计的姿势，通过“wall-1”和“wall-2”的阶段分别用蓝色和黄色阴影表示。估计的协方差也显示在图 8 中，它在整个估计轨迹上有界，表明我们的估计质量在整个过程中是稳定的。传感器移动到起始点，在那里使用 ArUco 标记板获取起始和结束姿势之间的真实相对姿势。与地面真实端位姿相比，我们的算法旋转漂移 1.62°，平移漂移 4.57 厘米。
我们在香港科技大学 (HKUST) 校园内以不同的行驶轨迹（即 Traj 1-4）收集了 4 次数据，它们的总长度分别为 1317、1524、1372 和 1191 米。这些轨迹的鸟瞰图（即在 X´Y 平面上的投影）如图 10 所示，它们的高度变化如图 11 所示。没有任何额外的处理（例如闭环），所有这四个轨迹都可以闭环（见图9（e））。使用放置在起点的 ArUco 标记板，里程计漂移如表 II 所示，这表明我们提出的方法具有高精度，在长轨迹和复杂环境中漂移很小。最后，我们在图 9 中的“Traj-1”中展示了重建的地图。项目页面上提供了更多可视化结果。
我们将 R3LIVE 估计的轨迹与两种不同的配置（“R3LIVE-HiRES”和“R3LIVERT”，见表 III）、“LVI-SAM”（为 Livox Avia LiDAR 修改其 LiDAR 前端）、“R2LIVE”进行比较 [12]、“VINSMono”（IMU+相机）[26]、“Fast-LIO2”（IMU+LiDAR）[22] 与图 12 中的真实情况，我们可以看到我们估计的轨迹最符合 两个序列中的真实情况。为了进行更多的定量比较，我们计算了所有可能的长度为 (50,100,150,...,300) 米的子序列的相对旋转误差 (RPE) 和相对平移误差 (RTE) [27]，如表 III 所示。
我们调查了我们系统在两个不同平台上的所有实验的平均时间消耗：台式机（具有 Intel i7-9700K CPU 和 32GB RAM）和无人机机载计算机（“OB”，具有 Intel i7-8550u CPU 和 8GB 内存）。详细统计数据列于表四。我们的 VIO 子系统的时间消耗受两个主要设置的影响：图像分辨率和点云图分辨率（“Pt res”）。
在 R3LIVE 实时重建彩色 3D 地图的同时，我们还开发了软件实用程序来离线对重建的地图进行网格划分和纹理化（见图 13）。对于网格划分，我们使用了在 CGAL [29] 中实现的 Delaunay 三角剖分和图切割 [28]。网格构建后，我们使用顶点颜色对网格进行纹理化，由我们的 VIO 子系统渲染。
我们开发的实用程序还可以将 R3LIVE 的彩色点图或离线网格图导出为常用的文件格式，如“pcd”、“ply”、“obj”等。因此，R3LIVE 重建的地图可以通过 各种 3D 软件，包括但不限于 CloudCompare [30]、Meshlab [31]、AutoDesk 3ds Max 等。
借助开发的软件实用程序，我们可以将重建的 3D 地图导出到 Unreal Engine 19 以启用一系列 3D 应用程序。例如，在图 14 中，我们使用 AirSim [32] 构建了汽车和无人机模拟器，在图 15 中，我们使用重建的地图为台式 PC 和移动平台开发视频游戏。有关我们演示的更多详细信息，我们建议读者在 YoutuBe 上观看我们的视频。
作者也是我们特邀嘉宾：
3D视觉工坊是基于的自媒体平台，创始人和合伙人致力于发布3D视觉领域最干货的文章，然而少数人的力量毕竟有限，知识盲区和领域漏洞依然存在。为了能够更好地展示领域知识，现向全体粉丝以及阅读者征稿，如果您的文章是、、、、、欢迎砸稿过来~文章内容可以为等形式，公众号将会对每一个投稿者提供相应的，我们支持知识有价！
邮箱：vision3d@yeah.net 或者加下方的小助理微信，另请注明原创投稿。
▲长按加微信联系
▲长按关注公众号