Stereo Visual Inertial LiDAR Simultaneous Localization and Mapping

Abstract

Simultaneous Localization and Mapping (SLAM) is a fundamental task to mobile and aerial robotics. LiDAR based systems have proven to be superior compared to vision based systems due to its accuracy and robustness. In spite of its superiority, pure LiDAR based systems fail in certain degenerate cases like traveling through a tunnel. We propose Stereo Visual Inertial LiDAR (VIL) SLAM that performs better on these degenerate cases and has comparable performance on all other cases. VIL-SLAM accomplishes this by incorporating tightly-coupled stereo visual inertial odometry (VIO) with LiDAR mapping and LiDAR enhanced visual loop closure. The system generates loop-closure corrected 6-DOF LiDAR poses in real-time and 1cm voxel dense maps near real-time. VIL-SLAM demonstrates improved accuracy and robustness compared to state-of-the-art LiDAR methods.
摘要：同时定位和地图绘制（SLAM）是移动和空中机器人技术的一项基本任务。与基于视觉的系统相比，基于LiDAR的系统在准确性和鲁棒性已被证明是更优的。尽管基于雷达系统更优，但是基于纯LiDAR的系统在某些退化的情况下仍无法正常工作，例如穿越隧道。我们提出了立体视觉惯性激光雷达（VIL）SLAM，在这些退化情况下性能更好，并且在所有其他情况下具有良好的性能。 VIL-SLAM通过将紧耦合的立体视觉惯性里程计（VIO）与LiDAR建图和LiDAR增强的视觉环路闭合功能结合在一起，实现了这一目标。该系统实时生成闭环校正的6自由度LiDAR姿态，并实时生成1cm体素密集图。与最优的LiDAR方法相比，VIL-SLAM展示出更高的准确性和鲁棒性。

系统概述

该系统具有四个模块，如图2所示。视觉前端从stereo相机获取stereo pairs。它执行帧到帧跟踪和立体匹配，并输出立体匹配作为视觉测量。双目VIO将立体匹配和IMU量测在位姿图（pose graph）上执行IMU预积分和紧耦合的固定滞后平滑。该模块以IMU速率和相机速率输出VIO位姿。LiDAR建图模块使用来自VIO的运动估计并执行LiDAR去畸变和scan-to-map配准。闭环模块执行视觉环路检测和初始环路约束估计，并通过稀疏点云ICP对齐进一步完善。约束所有LiDAR姿势的全局姿势图将得到逐步优化，以实时获取全局校正的轨迹和LiDAR姿势校正。
在这里插入图片描述

视觉前端和双目视觉惯性里程计

我们使用基于特征的方法进行匹配，该方法比KLT方法更适合处理large baselines。当被跟踪的stereo matches低于一个阈值时，前端基于Shi-Tomashi角点检测器进行特征提取。筛选特征后，对剩下的特征进行ORB特征计算，然后进行暴力匹配得到新的stereo matches，系统在第一个stereo pair进行stereo matching完成初始化。
紧耦合固定序列平滑位姿图在精度和效率之间取得了平衡，VIO包括三个过程：IMU预积分、无结构视觉因子、优化和边缘化。在这里插入图片描述

LiDAR建图和闭环

建图部分包括两个模块：LiDAR scan去畸变和scan-to-map配准。

闭环模块包括闭环检测和闭环约束两个部分。闭环对任何SLAM系统消除长时间累积漂移有至关重要的作用。其目的是通过对包括了环路约束和相对转换信息的全局位姿图进行优化来消除漂移。为了更好的辅助LiDAR建图，修正后的位姿被实时传回因此新的scan可以在重新访问的地图上进行配准。在词袋环路检测和PnP闭环约束之外，本文引入ICP对齐方法。系统使用iSAM2，一个增量求解器来优化全局位姿以保证算法的实时性。
在这里插入图片描述

实验

在这里插入图片描述

在这里插入图片描述
实验主要对比了不同场景以及不同和其他主流方法的对比，VIL-SLAM系统在精度和鲁棒性上均有良好表现。

A GPS-aided Omnidirectional Visual-Inertial State Estimator in Ubiquitous Environments

Abstract

The visual-inertial navigation system (VINS) has been a practical approach for state estimation in recent years. In this paper, we propose a general GPS-aided omnidirectional visual-inertial state estimator capable of operating in ubiquitous environments and platforms. Our system consists of two parts: 1) the pre-processing of omnidirectional cameras, IMU, and GPS measurements, and 2) the sliding window based nonlinear optimization for accurate state estimation. We test our system in different conditions including an indoor office, campus roads, and challenging open water surface. Experiment results demonstrate the high accuracy of our approach than state-of-the-art VINSs in all scenarios. The proposed odometry achieves drift ratio less than 0.5% in 1200 m length outdoors campus road in overexposure conditions and 0.65% in open water surface, without a loop closure, compared with a centimeter accuracy GPS reference.
摘要：视觉惯性导航系统（visual-inertial navigation system，VINS）是近年来发展起来的一种实用的状态估计方法。在这篇论文中，我们提出了一种通用的GPS辅助全向视觉惯性状态估计器，它能够在各种环境和平台上工作。该系统由两部分组成：1）全向相机、IMU和GPS量测的预处理；2）基于滑动窗口的非线性优化精确状态估计。我们在不同的条件测试了该系统，包括室内办公室，校园道路和具有挑战性的开阔水面。实验结果表明，在所有情况下，我们方法的精度高于最先进的VINSs。所提出的里程计在1200米长的室外校园道路中，在过曝条件下的漂移率小于0.5%，在开放水面上的漂移率为0.65%，在没有环路闭合的情况下，与厘米精度的GPS基准相当。

主要贡献

（1）一个精确和鲁棒的全向视觉惯性系统，具有灵活数量的针孔相机配置，在线相机-IMU准，和从非平稳状态快速初始化；
（2）紧耦合的非线性视觉和惯性测量优化与松耦合GPS生成精确里程计；（3）多种跨平台实验，包括室内，具有挑战性的室外地面和开放的水环境，具有优秀的性能。
在这里插入图片描述

算法流程

量测预处理

我们的系统有三种量测：图像稀疏特征、IMU和GPS测量。对图像和IMU测量数据进行预处理，然后合并到估计中。滑动窗口在图像频率上运行，低频GPS测量与同一时间戳图像绑定。（1）多相机的视觉特征提取：corner特征由Harris corner detect，跟踪由KLT稀疏光流算法；（2）IMU预积分，详见VINS-Mono。

系统初始化

（1）全向相机初始化；（2）从world frame到global frame的旋转定义。

外点剔除

可靠的视觉量测仅来自附近的静态对象。它们可以帮助校准参数快速地收敛并提供状态参数强约束。从海面，天空，和遥远的岛屿匹配到的特征会对估计产生负面影响而被记作不可靠的特征。我们定义不一致匹配和不可靠的特征作为异常值。首先在fundamental matrix上使用2D-2D的RANSAC，然后在3D特征位置上使用3D-2D PnP RANSAC外点剔除，在滑窗和2D相机平面完成。在优化模型中使用Huber loss来减少大残差外点造成的影响。最后我们在每次优化迭代后移除不合理的深度值。

非线性优化

我们提出了一个基于滑窗的非线性优化得到实时状态估计。（1）定义；（2）全向多相机量测模型；（3）GPS量测模型。
在这里插入图片描述

实验结果：

在这里插入图片描述

室内实验
将本文提出的算法和MSCKF-VIO，OKVIS和VINS-FUSION进行对比，结果如table2.
室外地面实验
Figure4和table3给出了室外地面实验结果，真值参考为RTK-GPS。

3室外开放水面实验
实验结果如figure5，同时对比了该算法在不同配置下的性能。
控制变量对比实验