多传感器融合SLAM：激光雷达、RGB相机、IMU定位与光度重建
作者 | 流川峰 编辑 | 深蓝前沿
点击下方卡片，关注“自动驾驶之心”公众号
点击进入→自动驾驶之心【SLAM】技术交流群
后台回复【SLAM综述】获取视觉SLAM、激光SLAM、RGBD-SLAM等多篇综述！
概述
分享一篇多传感融合定位的工作：R3live++。这是继R3live后的第二个版本。这项工作提出的激光雷达视觉惯性融合框架实现了鲁棒和准确的状态估计，同时实时重建光度地图。
系统由激光雷达惯性里程计 (LIO) 和视觉惯性里程计 (VIO) 组成，两者都是实时运行的。LIO子系统利用来自激光雷达的测量值来重建几何结构，而VIO子系统同时从输入图像中恢复几何结构的光度信息。通过考虑相机光度校准和相机曝光时间的在线估计，进一步提高了定位和映射的准确性。在公共和私有数据集上进行了更广泛的实验，将提出的系统与其他最先进的SLAM系统进行比较。定量和定性结果表明，R3LIVE++在准确性和鲁棒性方面均具有显着提高。
此外，为了演示R3LIVE的可扩展性，我们基于重建的地图开发了几种应用程序，例如高动态范围 (HDR) 成像，虚拟环境探索和3D视频游戏。代码是开源的，文末附源码和文章链接。
图1 R3live++系统结构
为了同时估计传感器姿态并重建环境辐射图，设计了一个紧密耦合的LiDAR-惯性-视觉传感器融合框架，如图1所示。
所提出的框架包含两个子系统: LIO子系统 (上部) 和VIO子系统。LIO子系统通过配准每个输入激光雷达扫描的点云测量来构造辐射图的几何结构。VIO子系统通过两个步骤恢复地图的辐射信息: 帧到帧VIO更新通过最小化帧到帧PnP重投影误差来估计系统状态，而帧到地图VIO更新最小化地图点和当前图像之间的光度误差。
这两个子系统在流形错误状态迭代卡尔曼滤波器框架 (ESIKF) 内紧密耦合，其中激光雷达和摄像机视觉测量在其各自的数据接收时间融合到相同的系统状态 。
我们的辐射图由全局框架中的地图点组成，每个点P是如下结构:
其中前三维表示点3D位置，并且后三维是由三个独立通道 (即红色，绿色和蓝色通道)，用于相机光度校准。此外，我们还记录该点的其他必要信息，包括点的估计误差的协方差，以及创建和更新该点时的时间戳。
图2 彩色图像成像过程
相机以图像的形式观察现实世界的辐射，该图像由像素强度的2D阵列组成。对相机的图像形成过程进行建模，并进一步将灰色相机模型扩展到彩色相机。如图2所示，对于世界上的点P，它反射从光源 (例如，太阳) 发出的入射光。然后，反射光穿过相机镜头，最终到达CMOS传感器，该传感器记录反射光的强度，并在输出图像中创建一个像素通道。记录的强度由辐射度确定，辐射度是在点P处反射的功率的度量。
为了模拟上述成像过程，P处的辐射度表示为 r。由于彩色相机的CMOS传感器具有三个通道: 红色，绿色和蓝色，对于每个通道 i，穿过相机镜头的光线都有功率
其中V(p)被称为解释透镜渐晕效应的渐晕因子。由于渐晕效果在透镜的不同区域处是不同的，因此渐晕因子V(p) 是像素位置 p可以接收的功率量，称为辐照度。当拍摄图像时，捕获的辐照度O(p) 随时间 (即曝光时间r) 积分。累积辐照度通过相机响应函数 (CRF) ，转换为像素强度Ij(p) 的输出:
曝光时间 r 在我们的工作中是在线估计的。校准和估计结果,从观察到的像素值 I (P) 得出的点 p 的辐射度可以计算为:
在我们的工作中，我们将完整状态x定义为:
其中符号 Itc是IMU和相机之间的时间偏移，而LiDAR被假定已经与IMU同步。相机外参和时间偏移 tc 通常具有可用的粗略值 。外参或时间延迟包括在状态x中，以便在线估计它们。此外，我们还在线估算相机曝光时间，以恢复每个地图点的真实辐射值。
a.雷达惯性里程计 (LIO)
LIO 子系统通过将每个新的 LiDAR 扫描注册到全局地图来重建环境的几何结构。使用广义迭代最近点 (GICP) 方法,通过最小化扫描中每一点到从地图中相应点拟合的平面的距离,迭代估计激光雷达姿态 (和其他系统状态)。然后使用估计的状态估计将新点附加到地图上。激光雷达点对平面残差可以写为：
想情况下，残差应该为零。但是，由于X中的估计误差和激光雷达测量噪声，该残差通常不为零，我们使用ESIKF细化状态估计X。
b.视觉惯性子系统VIO
LIO 子系统重建环境的几何结构时,我们的 VIO 子系统从输入的彩色图像中恢复辐射信息。更具体地说,我们的 VIO子系统将一定数量的点 (即,跟踪点) 从全局地图投影到当前图像,然后通过最小化这些点的辐射误差来迭代估计相机姿势 (和其他系统状态)。
为了提高计算效率,仅使用一组稀疏的跟踪地图点。我们提出的框架不同于以前的基于光度的方法 ,后者通过考虑所有邻域像素 (即补丁) 的光度误差来构成点的残差。这些基于补丁的方法比没有补丁的方法具有更强的鲁棒性和更快的收敛速度。但是,基于补丁的方法对于平移或旋转都不是不变的,这需要在将一个补丁与另一个补丁对齐时估计相对变换。另外,通过假设补丁中所有像素的深度与中点相同,残差的计算并不完全精确。
另一方面,我们的 VIO 在单个像素上运行,该像素利用单个地图点的辐射来计算残差。在 VIO 中同时更新的轮胎辐射度是世界上一个点的固有属性,并且对于相机的平移和旋转都是不变的。为了确保稳健和快速的收敛,我们设计了一个两步流水线,如图所示,其中在第一步 (即,帧到帧 VIO) 我们利用帧到帧光流来跟踪最后一帧中观察到的地图点,并通过最小化被跟踪点的透视 n 点 (PnP) 重投影误差来获得系统状态的粗略估计 。
然后,在第二步骤 (即帧到地图 VIO) 中,通过最小化地图点的辐射度与当前图像中它们的投影位置处的像素强度之间的差来进一步细化状态估计 。利用收敛状态估计和原始输入图像,我们最终根据当前图像测量更新地图点的辐射度 。
图3 辐射度的贝叶斯更新
i.辐射信息的恢复
点到地图VIO更新后，我们将获得当前图像的精确姿势。然后，我们执行贝叶斯更新以确定所有地图点的最佳辐射度，以使每个点与其查看图像之间的平均辐射度误差最小。见图3。
c.实验
在本章中，我们进行了广泛的实验，以验证我们提出的系统相对于其他同行的优势有三个方面:
1) 为了验证本地化的准确性，我们将系统与公共数据集 (NCLT) 上现有的最先进的SLAM系统进行了定量比较。
2) 为了验证我们框架的鲁棒性，我们在发生相机和激光雷达传感器退化的各种具有挑战性的场景下对其进行了测试。
3) 为了评估我们的系统在重建辐射图方面的准确性，我们将其与现有的基线进行比较，以估算相机曝光时间并计算平均光度误差。
图4 激光雷达退化环境中的测试
i.辐射图重建的评估
在这个实验中，我们评估了我们提出的算法在重建辐射图方面的准确性。由于无法测量环境的地面真实辐照度图，因此我们根据两个指标评估精度: 一是相机曝光时间的估计质量，二是重建的辐照度图与测量图像之间的平均光度误差。
文章链接：https://arxiv.org/abs/2209.03666
【自动驾驶之心】全栈技术交流群