一种R3LIVE++的LiDAR惯性视觉融合框架

摘要

本文提出了一种名为 R3LIVE++ 的 LiDAR 惯性视觉融合框架，以实现稳健且准确的状态估计，同时实时重建辐射图。R3LIVE++ 由实时运行的 LiDAR 惯性里程计 (LIO) 和视觉惯性里程计 (VIO) 组成。LIO 子系统利用来自 LiDAR 的测量来重建几何结构，而 VIO 子系统同时从输入图像中恢复几何结构的辐射信息。

R3LIVE++基于R3LIVE，通过结合相机光度校准和相机曝光时间的在线估计，进一步提高了定位和建图的准确性。论文在公开和私有数据集上展开了充分的实验，并同其他SOTA的SLAM 系统进行比较。定量和定性的结果表明，R3LIVE++ 在准确性和鲁棒性方面都有很大的优势。

此外，为了展示R3LIVE++ 的可扩展性，论文基于R3LIVE++得到的重建地图开发了若干应用程序，例如高动态范围 (HDR) 成像、虚拟环境探索和 3D 视频游戏。 在SLAM中融合LiDAR和相机测量，可以克服各个传感器在定位中的退化问题，并生成满足各种地图应用需求的精确、纹理化和高分辨率3D地图。基于此，论文提出的R3LIVE++有以下特点：

R3LIVE++是一个激光雷达惯性视觉融合框架，紧密耦合了两个子系统：LIO和VIO子系统。这两个子系统联合增量地实时构建周围环境的3D辐射图。特别是，LIO通过将LiDAR中的新点云配准到地图中来重建几何结构，而VIO则通过将图像中的像素颜色渲染到地图中的点来恢复辐射信息；

R3LIVE++提出一个新颖的VIO设计，通过最小化辐射图中的点与当前图像中稀疏像素集合间的辐射度差异来跟踪相机姿态（并估计其他系统状态）。frame-to-map的对齐有效地降低了里程计的漂移，而单个像素稀疏集合上的直接光度误差有效地限制了计算负载。此外，基于光度误差，VIO能够在线估计相机曝光时间，从而能够恢复环境的真实辐射信息；

在定位精度、鲁棒性和辐射图重建精度方面，论文的方法在真实世界的实验中得到了广泛验证。来自开放数据集（NCLT数据集）的25个序列的基准测试结果表明，相比之下，R3LIVE++在所有其他最先进的SLAM系统（如LVI-SAM、LIO-SAM、FAST-LIO2等）中实现了新SOTA。

此外，论文在私有数据集的评估表明，R3LIVE++对于激光雷达或相机测量退化的极端挑战场景（例如，当设备面对无纹理的墙时）具有更好的鲁棒性。最后，与其他同类产品相比，R3LIVE++可以更准确地估计相机的曝光时间，并重建环境的真实辐射信息，与图像中的测量像素相比，误差要小得多；

据论文称，这是第一个辐射图重建框架，它可以在配备标准CPU的PC上实现实时性能，而无需任何硬件或GPU加速。基于进一步开发的一套网格重建和纹理处理离线应用程序，该系统在各种现实应用中显示出巨大潜力，例如3D HDR成像、物理模拟和视频游戏。

方法

论文使用的符号含义如下表所示：

系统概览

为了同时估计传感器姿态并重建环境辐射图，论文设计了一个紧密耦合的LiDAR惯性视觉传感器融合框架，如图2所示。该框架包含两个子系统：LIO和VIO子系统。LIO子系统通过记录每个输入LiDAR的点云测量值，构建辐射图的几何结构。

VIO子系统分两步恢复地图的辐射信息：帧到帧VIO更新通过最小化帧到帧PnP重投影误差来估计系统状态，而帧到地图VIO更新则最小化地图点云和当前图像之间的辐射误差。这两个子系统在流形误差状态迭代卡尔曼滤波器框架（ESIKF）[43]内紧密耦合，其中激光雷达和摄像机视觉测量在各自的数据接收时间融合到相同的系统状态。

点

本文的辐射图由全局框架中的地图点组成，每个点P的结构如下所示：

体素

为了有效地查询辐射图中的点，论文将地图点设置为固定大小（0.1m×0.1m×0.1m）的体素。如果一个体素近期附加了点（例如最近1秒），则将该体素标记为激活。否则，此体素将标记为停用。

彩色相机光度模型

相机以图像的形式观察现实世界的辐射，图像由像素强度的2D数组组成。在本文中基于[44]对相机的图像形成过程进行建模，并将灰色相机模型进一步扩展到彩色相机。如图3所示，对于现实世界中的点P，它反射来自光源（例如太阳）的入射光。反射光通过相机镜头，最终到达CMOS传感器，传感器记录反射光的强度，并在输出图像中创建像素通道。记录的强度由辐射度决定，辐射度是点P反射功率的测量值。



为了建模上述成像过程，使用表示点P的辐射度。由于彩色相机的CMOS传感器中有三个通道：红色、绿色和蓝色，因此辐射度有三个分量：、、 。对于每个通道，穿过相机镜头的光线有如下能量：



拍摄图像时，捕获的辐射度随时间（即曝光时间）进行积分。然后，通过相机响应函数将累积辐射度转换为像素强度输出：



由于真实的相机传感器具有有限的动态范围，并且辐射的物理尺度无法恢复，因此可以将像素强度归一化到0-1之间。 不同通道通常具有不同的非线性响应函数，它们可以根据[45]中的方法与vignetting factor一起离线校准。本文的曝光时间是在线估计的。根据校准和估计结果，点P相对于观测像素值的辐射度可计算为：

状态

最终状态x定义如下：

激光雷达惯性里程计（LIO）

论文的LIO子系统通过将新的LiDAR点云配准到全局地图来重建环境的几何结构。即使用generalized-iterative closet point（GICP）方法[46]，通过最小化扫描中每个点云到地图中相应点拟合的平面的距离，迭代估计LiDAR姿态（和其他系统状态）。然后使用估计的状态估计值将新点附加到地图。

LiDAR point-to-plane residual

如图2所示，论文的LIO子系统构建了全局地图的几何结构。对于第k个输入LiDAR扫描，首先使用在[22]中引入的IMU反向传播来补偿帧内运动。假设是运动补偿后m个LiDAR点的集合，计算的每个原始点（或下采样子集）的残差，其中s是点的索引，上标L表示点在LiDAR参考帧中的表示。 基于当前迭代的估计，将从LiDAR帧转换到全局帧：



为了将点云配准到全局地图，论文搜索地图中最近的五个点。为了加速最近邻搜索，地图点被组织成incremental k-d tree（参见[23]）。使用找到的最近邻点拟合一个具有法向和质心的平面，然后使用激光雷达测量残差：

LIO ESIKF update

理想情况下，上式中的残差应为零。然而，由于中的估计误差和LiDAR测量噪声，该残差通常不是零，可以用于细化状态估计。具体来说，假设是点的测量噪声，则真值和测量值的关系如下所示：



该真实点位置和真实状态可以推导出下式：



上式构成的观测分布，它与IMU传播的先验分布相结合得到：



求解上式得到的Maximum A-Posteriori（MAP）估计值，然后更新到中，如下所示：



迭代直至收敛（即更新小于给定阈值）。然后将收敛状态估计值用作IMU传播的起点，直到接收到下一个LiDAR扫描或相机图像。此外，收敛估计值用于将当前LiDAR扫描中的点添加到全局地图中，如下所示。对于第s个点，其在全局帧中的位置首先通过（6）获得。如果在地图中有距离为1cm的附近点，将被丢弃，以保持1cm的空间分辨率。否则，将在地图中创建一个新的点结构：

视觉惯性里程计(VIO)

当LIO子系统重建环境的几何结构时，VIO子系统从输入的彩色图像中恢复辐射信息。更具体地说，VIO子系统从全局地图向当前图像投影一定数量的点（即跟踪点），然后通过最小化这些点的辐射误差来迭代估计相机姿态（和其他系统状态）。为了提高计算效率，只使用一组稀疏的跟踪地图点。

论文提出的框架不同于以前基于光度的方法[33]、[47]，这些方法通过考虑所有相邻像素（即patch）的光度误差来构成点的残差。这些基于patch的方法具有更强的鲁棒性和更快的收敛速度，然而对平移或旋转都不是不变的，即需要在两个patch对齐时估计相对变换。

此外，这些方法假设patch中所有像素的深度与中心点相同，所以残差的计算并不精确。另一方面，论文的VIO在逐像素上操作，利用单个地图点的辐射来计算残差。在VIO中同时更新的辐射度是世界上某个点的固有属性，对摄影机平移和旋转都是不变的。

为了确保稳健和快速收敛，论文设计了图2所示的两步流水线，其中在第一步（即 frame-to-frame VIO）中，利用frame-to-frame光流跟踪最后一帧中观察到的地图点，并通过最小化跟踪点的PnP重投影误差来获得系统状态的粗略估计。然后，在第二步（即frame-to-map VIO）中，通过最小化地图点的亮度和当前图像中其投影位置的像素强度之间的差异，进一步细化状态估计。

通过收敛状态估计和原始输入图像，论文最终根据当前图像测量更新地图点的辐射度。

光度矫正

对于输入图像，首先校正图像non-linear CRF 和vignetting factor ，其已经预先校准，以获得测光校正图像，其像素位置的第i通道为：



测光校正图像 之后在VIO管道中使用，包括frame-to-frame VIO、frame-to-map VIO和辐射恢复。

Frame-to-frame VIO

Perspective-n-Point reprojection error

假设跟踪了m个在最后一帧图像中的地图点及其在图像中的投影位置。论文使用Lucas Kanade光流来定位这些点在当前图像中的位置。然后迭代地最小化点P的重投影误差，以获得状态的粗略估计（见图4）。



投影误差表示如下：

Frame-to-frame VIO Update

与LIO更新类似，中的状态估计误差和相机测量噪声将导致（14）中的某个残差，从中可以如下更新状态估计。首先（14）中的测量噪声由两个来源组成：一个是的像素跟踪误差，另一个是地图点定位误差：



类似的，校正此类噪声并使用真实的系统状态应导致零残差：



上式构成了的观测分布，将其与与IMU传播相结合，以获得状态的MAP估计值，方法与LIO更新相同。然后收敛的状态估计值用于细化frame-to-map VIO。

Frame-to-map VIO

Frame-to-map radiance error

frame-to-frame VIO更新可以提供良好的状态估计，通过最小化跟踪的地图点P的辐射误差，frame-to-map VIO更新会进一步细化。利用当前迭代的状态估计（包含估计的相机姿态、内外参和曝光时间），进一步将跟踪的地图点投影到图像平面，以获得其像素位置。然后，由表示的观测辐射度可以由下式获得：



最后，frame-to-map辐射误差由下式获得：

Frame-to-map VIO update

上式中的测量噪声来自辐射分量和：






结合（19）、（20）和（21）进一步得到了真实零残差的一阶泰勒展开式：



与之前类似，（22）构成了状态的观测分布，该分布与IMU传播相结合，以获得该状态的MAP估计值。

Recovery of radiance information

在frame-to-map VIO更新后，便得到了当前图像的精确姿态。然后进行贝叶斯更新以确定所有地图点的最佳辐射度，从而使每个点与其对应图像之间的平均辐射度误差最小。 进一步检索所有激活体素中的点。假设检索到的点集为Q。对于落在当前图像FoV中的第s个点，首先通过（19）获取观测到的辐射向量，通过（21）获得其协方差。然后，如果是LIO子系统附加的一个新点且，则我们设置：



否则，通过贝叶斯更新（见图6），将保存在地图中的辐射矢量与新观测到的辐射矢量进行融合：

Update of the tracking points

在恢复辐射信息后，论文更新跟踪点集P以供下一帧图像使用。首先，如果（14）中的投影误差或（19）中的辐射误差过大，就从当前P中删除点，并删除不属于当前图像FoV的点。其次，论文将Q中的每个点投影到当前图像中，如果在50像素的邻域中没有其他跟踪点，则将其添加到P中。

实验

论文进行了大量实验，从三个方面验证了提出的系统相对于其他同类系统的优势：

1）为了验证定位的准确性，在公开数据集（NCLT数据集）上定量比较了R3LIVE++与其他SOTA的SLAM系统；

2） 为了验证框架的鲁棒性，在摄像机和激光雷达传感器退化的各种具有挑战性的场景下进行测试；

3） 为了评估R3LIVE++在重建辐射图时的精度，论文将其与现有基线进行比较，以估算相机曝光时间并计算每个图像的平均光度误差。在实验中，使用了两个数据集进行评估：NCLT数据集和私有的R3LIVE数据集。

R3LIVE数据集收集于香港大学（HKU）和香港科技大学（HKUST）的校园内。如表2所示，数据集包含13个序列，通过探索室内和室外环境，在一天中不同时间（即上午、中午和晚上）的不同场景（如人行道、公园、森林等）中收集这些序列。这使得数据集能够捕获具有不同照明条件下的结构化城市建筑和杂乱现场环境。

数据集还包括三个退化序列，其中激光雷达或相机（或两者）偶尔面对设备，使其退化为单个视觉或无纹理平面（例如，墙、地面）。总行程8.4km，持续时间达到2.4h。

定位准确性评估

论文将R3LIVE++与其他SLAM系统进行对比，包括LIO-SAM[19]、LVI-SAM[40]、FAST-LIO2[23]和R2LIVE[41]。 对比结果如下表所示：


在图10中，论文对比叠加了所有25条地面真实轨迹（左图）和R3LIVE的结果（右图）。可以看出，R3LIVE++系统估计的叠加轨迹与GT吻合得很好，并且每个轨迹仍然可以清楚地区分，没有明显的误差。请注意，这25条轨迹是在同一校园区域内一天中的不同时间和一年中的不同季节采集的，但R3LIVE++仍然可以生成一致且可靠的轨迹估计，证明了系统的鲁棒性。

鲁棒性评估

LiDAR退化场景下的鲁棒性评估：可以看出，由于面对单个平面时激光雷达退化，所有三个LIO系统都失败了，没有返回起点。相比之下，通过利用视觉图像中的线索，R3LIVE++在这两个序列中都能很好地工作，并成功地回到起点。

LiDAR退化和视觉纹理较少环境下的鲁棒性评估：在这个实验中，R3LIVE++挑战了SLAM中最困难的场景之一，激光雷达和相机都会退化。传感器装置穿过一个狭窄的“T”形通道（见图11）



利用原始像素颜色信息，并将其与LiDAR点云测量紧密融合，R3LIVE++可以在这种极其困难的场景中“生存”。图12是姿态估计的结果，穿过“墙-1”和“墙-2”的阶段分别用紫色和黄色阴影表示。估计协方差也如图12所示，它在整个估计轨迹上有界，表明R3LIVE++的估计质量在整个过程中是稳定的。

辐射图重建评估

由于无法测量环境的真实辐射图，论文使用两个指标来评估精度：一个是相机曝光时间的估计质量，另一个是重建的辐射图与测量图像之间的平均光度误差。

曝光时间估计：在这个实验中，论文通过将估计的相机曝光时间与从相机API读取的GT进行比较来评估其准确性。结果如图13和表4所示。



辐射图评估：为了评估辐射重建误差，在地图重建后使用估计的相机姿态和校准的光度参数，将地图中所有具有辐射信息的点重新投影到每个图像帧。然后计算地图点颜色和图像在投影像素位置的RGB值之间的光度误差。计算每个图像帧的平均光度误差，以评估重建的辐射图精度。实验结果如图14和表5所示：


可以看出，R3LIVE++在所有序列中始终实现了最低的光度误差，其次是R3LIVE。图15显示了重建辐射图的一些特写镜头，从中可以清楚地分辨物体上的单词。此外，图16展示了R3LIVE数据集中“香港大学主楼”序列的重建辐射图，室内和室外细节（例如道路上的标记）都非常清晰，表明R3LIVE++具有较高的精度。

运行时间分析

本节分析SLAM系统在纯CPU PC（配备Intel i7-9700K CPU和64GB RAM）上的平均耗时。论文计算了两个数据集所有序列的平均耗时，结果如表6和表7所示。在NCLT数据集上，每次LiDAR扫描需要34.3 ms，相机需要16.6 ms的处理时间。

R3LIVE++的应用

High dynamic range (HDR) imaging

在重建辐射图后，可以进一步渲染图像，可视化结果如图17所示：

Mesh reconstruction and texturing

当R3LIVE++实时重建彩色3D地图时，论文还开发了软件程序来离线对重建的地图进行网格划分和纹理处理（见图18）。

Toward various of 3D applications

通过开发的软件，论文将将重建的3D地图导出到Unreal Engine[60]，以支持一系列3D应用程序。如图19所示的汽车模拟器、视频游戏等

结论和未来工作

结论

论文提出了一种新的激光雷达惯性视觉融合框架R3LIVE++，以实现鲁棒和精确的状态估计，同时动态重建辐射图。该框架由两个子系统（即LIO和VIO）组成，这两个子系统共同增量地实时构建环境的3D辐射图。通过将三种不同类型传感器的测量值紧密融合，R3LIVE++可以实现更高的定位精度，同时对传感器退化的场景具有足够的鲁棒性。

论文通过真实世界的实验，在定位精度、鲁棒性和辐射图重建精度方面验证了R3LIVE++的有效性。在NCLT数据集上的实验结果表明，R3LIVE++超越了以往SOTA的SLAM系统。对私有数据集（R3LIVE数据集）的评估表明，R3LIVE++对LiDAR或相机测量退化的极端挑战性场景具有鲁棒性。最后与其他算法相比，R3LIVE++可以更准确地估计相机的曝光时间，并重建环境的真实辐射信息，与图像中的测量值相比，误差要小得多。

为了证明R3LIVE++的可扩展性，论文基于重建的辐射图开发了几个应用程序，例如高动态范围（HDR）成像、虚拟环境探索和3D视频游戏。

未来工作

在R3LIVE++中，辐射图是用包含辐射信息的3D点云重建的，因此无法从辐射图中渲染高分辨率图像，因为辐射图的点云密度有限（论文设置为1cm）。虽然可以进一步增加点云密度，但也会进一步增加耗时。此外点云密度还受到激光雷达传感器测量的原始点云密度的限制。我们注意到图像通常具有更高的分辨率，未来可以探索如何在融合框架中充分利用此类高分辨率图像。

审核编辑：刘清