立体视觉+惯导+激光雷达SLAM系统

点击上方“3D视觉工坊”，选择“星标”

干货第一时间送达

标题：Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping

作者：Weizhao Shao, Srinivasan Vijayarangan∗, Cong Li∗, and George Kantor

来源：分享者

编辑：dianyunPCL

摘要

本文提出的立体视觉+惯导+激光雷达的SLAM系统，在比如隧道一些复杂场景下能够实现良好性能。VIL-SLAM通过将紧密耦合的立体视觉惯性里程计（VIO）与激光雷达建图和激光雷达增强视觉环路闭合相结合来实现这一目标。该系统实时生成环闭合校正的6自由度激光雷达姿态和接近实时的1cm体素稠密点云。与最先进的激光雷达方法相比，VIL-SLAM显示了更高的精确度和鲁棒性。

（a）传感器设备（b）构建室外场景模型

主要内容

VIL-SLAM系统图。传感器为灰色，模块为绿色。箭头指示消息在系统中的流动方式。深色粗箭头表示系统实时输出，浅色粗箭头表示近实时后处理生成的输出。

系统有四个模块，如图2所示。视觉前端从立体摄像机获取立体图像。它执行帧到帧的跟踪和立体帧匹配，并输出立体匹配结果作为视觉测量。立体VIO采用立体匹配和IMU测量，在位姿图上执行IMU预积分和平滑紧耦合结果。该模块根据IMU和摄像机输出VIO姿态。LiDAR建图模块使用VIO的运动估计，并执行LiDAR点去噪和扫描以进行地图配准。回环闭合模块进行视觉环路检测和初始环路约束估计，并通过稀疏点云ICP对准进一步精细配准。对约束所有LiDAR姿态的全局姿态图进行增量式优化，得到全局修正轨迹和实时LiDAR姿态修正。它们被送回激光雷达建图模块进行地图更新和重新定位。在后处理中，将激光雷达扫描帧与最佳估计的激光雷达姿态拼接，以获得稠密的点云地图结果

图6 实验装置使用两个VPs校准相机机

视觉前端

对立体视觉进行立体匹配，本文使用Kanade Lucas Tomasi（KLT）特征跟踪器来跟踪先前立体匹配中的所有特征点，无论是在左图像还是右图像中。只有当他们都被跟踪才会输出。较大的立体基线有助于尺度估计，并减少由远距离特征引起的退化问题。我们使用基于特征的方法，比KLT更适合处理长基线。如果跟踪的立体匹配数低于阈值，使用Shi-Tomashi角点检测器进行特征提取，然后进行特征消除过程，删除与任何小于阈值的现有特征的像素距离的特征。ORB描述子后计算所有幸存特征，然后进行暴力立体匹配以获得新的匹配结果。系统通过对第一帧立体视觉进行立体匹配来初始化系统。

视觉惯导里程计

立体VIO的目标是在相对较高的频率上提供实时准确的状态估计，作为LiDAR建图算法的运动模型。在位姿图上运行的紧耦合固定滞后平滑器在精度和效率之间是一个很好的权衡。基于优化的方法通常允许多次线性化以接近全局最小值。固定滞后位姿图优化器进一步限定了变量的最大数目，因此计算成本是有界的。由于糟糕的视觉测量会导致收敛问题，对视觉测量强制执行严格的异常值拒绝机制。系统通过检查平均重投影误差来消除异常值。

该算法以IMU预积分因子和无结构视觉因子作为约束条件

激光雷达建图

激光雷达建图在进行激光雷达点提取和扫描帧点云到地图配准之前，使用高频IMU速率VIO姿态作为运动。将扫描c表示为从一个完整的激光雷达旋转获得的点云。从c中提取几何特征，包括锐边上的点和平面上的点。然后，基于当前扫描到地图的特征点（所有先前的特征点），通过最小化特征点形成的欧几里德距离残差，将其作为一个优化问题来解决。

基于雷达增强的回环检测

回环检测对任何一个SLAM系统都是至关重要的，因为长期运行会引入漂移。环路闭合的目的是通过对全局位姿图进行优化来消除漂移，该全局位姿图结合了LiDAR建图的回环约束和相对变换信息。为了更好地辅助激光雷达制图，校正后的激光雷达姿态被实时发送回来，以便新扫描的特征点被配准到重新访问的地图上。本文提出在视觉词袋回环检测和PnP回环约束公式的基础上增加ICP对齐。该系统采用增量求解器iSAM2对全局姿态图进行优化，实现了实时性

实验结果

评估了VIL-SLAM，并将其与基于最佳实时激光雷达的系统LOAM2在自定义数据集上进行了比较。还使用EuRoC MAV数据集评估立体VIO子模块（VIL-VIO）。

这里提供了五个典型环境的结果，包括无特色的走廊、杂乱无章的高台、隧道和室外环境。所有这些序列的数据收集在同一点开始和结束。基于最终漂移误差（FDE）对里程计（LiDAR建图姿态）进行评估。以Faro扫描为基本真实值，用平均配准误差（MRE）对建图结果进行评估。首先将地图与模型对齐（Faro扫描），然后计算地图点与模型中最近点之间的欧几里德距离。里程表FDE和建图结果如表一所示，较好的用粗体显示。

地图比较：VIL-SLAM（右）和LOAM（左）与模型相比的地图配准误差。0.3m以上的误差（a-b）用红色表示，0.5m以上的误差（c）用红色表示。蓝色和绿色内部不连续的红色区域是由于扫描设备的遮挡导致的模型缺失。

总结

VIL-SLAM是一种先进的里程计和地图构建系统，旨在在不同的环境中长期稳定地运行。当前的框架将VIL-VIO和激光雷达建图松耦合在一起。我们正在将其扩展到一个紧密耦合的框架中，以便从LiDAR建图得到的精确姿态估计可以用于IMU偏差校正。在循环闭合中对扫描帧之间的稀疏特征点进行ICP精细化获得更好的回环约束。

本文仅做学术分享，如有侵权，请联系删文。

下载1

在「3D视觉工坊」公众号后台回复：3D视觉，即可下载 3D视觉相关资料干货，涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。

下载2

在「3D视觉工坊」公众号后台回复：3D视觉github资源汇总，即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。

下载3

在「3D视觉工坊」公众号后台回复：相机标定，即可下载独家相机标定学习课件与视频网址；后台回复：立体匹配，即可下载独家立体匹配学习课件与视频网址。