0. 简介

传统的地图生成方法一般是依靠Lidar和IMU结合的，但是问题在于，目前Lidar和IMU的紧耦合主要集中在前端里程计，基本没有涉及到后端全局优化以及建图的融合。为此文章《Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping》提出了一种改进的地图生成方法，并设计一个在所有阶段(前端里程计+后端全局优化和建图)，IMU和LIDAR都紧耦合的SLAM系统。这是作者实验室的官网，后续应该其后续工作应该会在官网中查到。

Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping

1. 文章贡献

本文的LiDAR-IMU紧耦合方法贯穿所有状态估计阶段。是目前为止第一个在全局轨迹规划上同时利用了LiDAR和IMU的约束的工作。与此前的LiDAR-IMU SLAM框架相比有以下几点不同：

不同于此前的LiDAR SLAM从点云中提取平面和边缘特征来进行匹配，本文使用体素化的GICP匹配代价因子（voxelized GICP matching cost factor），可以在GPU上并行计算；
本方法中的紧耦合里程计模块使用固定滞后平滑；
激光雷达和IMU的紧密耦合，这里的紧耦合不仅体现在前端，而且在后端的因子图优化中也大量体现了紧耦合。

2. 详细内容

系统框架如下图，主要包括4个模块，预处理模块，前端里程计模块，局部建图模块，全局建图模块，所有模块都是LIDAR-IMU紧耦合的。里程计估计(即前端)模块鲁棒地估计传感器运动，并提供最新传感器状态的初始估计。通过后续的局部映射模块对估计的传感器状态进行细化，并将多个局部帧合并为一个子地图。然后，全局映射模块优化子地图位姿，使全局配准误差最小化，同时保持地图的一致性。我们通过多线程并行运行这些模块。

将在估计模块中被估计的传感器状态 $x_t$ 定义为

$x_t = [T_t, v_t, b_t]^T$

其中 $T_t = [R_t| T_t]∈SE(3)$ 为传感器位姿， $vt∈R3v_t∈\mathbb{R}^3$ 为速度， $bt=[bta,btω]∈R6b_t = [b^a_t, b^ω_t]∈\mathbb{R}^6$ 为IMU加速度和角速度偏差。从激光雷达点云 $P_t$ 和IMU测量值(线性加速度 $a_t$ 和角速度 $ω_t$ )估计传感器状态的时间序列。请注意，我们将LiDAR点云转换为IMU坐标框架，并且为了效率和简单性，将它们视为统一的传感器坐标框架。

这部分的具体操作即为进行了一个离群点的剔除，只不过这里离群的定义是依赖于时间戳，由于激光雷达的扫描方法，落入同一个体素格子的点的时间戳应该是大差不差的，用这个时间来做剔除，从而减小噪声点对体素化的负面影响。

2.1 LIDAR匹配残差因子

匹配代价因子约束2个传感器位姿( $Ti\mathbf{T}_i$ 和 $Tj\mathbf{T}_j$ )，使点云( $Pi\mathcal{P}_i$ 和 $Pj\mathcal{P}_j$ )之间的匹配代价最小。选择voxelized GICP (VGICP)代价[20]作为匹配代价，它是适用于GPU计算的广义ICP[21]的变体。

VGICP将每个输入点 $pk∈Pip_k∈\mathcal{P}_i$ 建模为高斯分布 $p_k = (μ_k, C_k)$ ，根据当前点 $p_k$ 的邻近点计算协方差矩阵 $C_k$ ，而上一帧点云 $Pj\mathcal{P}_j$ ，假设已经经过建图模块融合了的话，则 $Pj\mathcal{P}_j$ 就已经被体素化，每个体素也是通过一个高斯分布来表示的，这个高斯分布是通过该体素内的所有点的高斯分布计算出来的，当前点云的 $p_k$ 点通过位姿变换：
$Ti−1Tj\mathbf{T}^{-1}_i\mathbf{T}_j$
上一帧点云中的某个体素也符合正态分布：
$p'_k = (μ'_k, C'_k)$
构成一组数据关联。如果我们把这组数据关联对应的残差记为
$dk=μk′−Ti−1Tjμkd_k=μ'_k-\mathbf{T}^{-1}_i\mathbf{T}_jμ_k$

然后，基于GICP分布到分布的距离定义 $Pi\mathcal{P}_i$ 和 $Pj\mathcal{P}_j$ 之间的匹配代价 $e^M$ ,点云利用体素来实现一个离散化，对落入体素内的点的分布情况进行处理，可以得到内部点的高斯分布，那么按照一般SLAM求位姿的思路，我们的流程大概率是列一个残差的式子，然后寻找一个位姿变化 $T$ 来最小化这个残差，在GICP里面作者使用的是下面这样一个残差的表示方法。

其中 $p'_k =(µ'_k, C'_k)$ 是通过查找 $Pj\mathcal{P}_j$ 的体素图得到的 $p_k$ 对应体素的均值和协方差， $d_k =µ'_k−T_{ij}µ_k$ 是 $µ_k$ 和 $µ'_k$ 之间的残差。

从公式2的导数中，我们得到一个Hessian因子来约束 $Ti\mathbf{T}_i$ 和 $Tj\mathbf{T}_j$ 之间的相对姿态。值得强调的是，在每次优化迭代中，都在当前线性化点重新评估和线性化 $e^M$ ， $e^M$ 就是GICP的成本因子，我们可以理解为是总的残差里面的一部分，就像VINS一样。从而得到比传统SE3相对位姿约束[4]更精确的约束。

2.2 IMU的预积分因子

利用IMU预积分技术[22]有效地将IMU约束融入因子图中。给定IMU测量值( $a_t$ 和 $ω_t$ )，在给定IMU测量的情况下，传感器的未来状态可以估计为：

IMU预积分因子整合了 $i$ 和 $j$ 两个时间步间的系统演化，得到了相对的身体运动约束(详细推导参见[22]):

IMU预积分因子使系统能够在几何特征不足且 LiDAR 因子可能不足的环境中保持因子图的良好约束。

3. SLAM pipeline

3.1 数据预处理

这部分中论文作者提到了一个偏移校正的方法，采用时间戳来进行校正。原文的意思个人感觉是给点和体素格子都赋一个时间戳，如果点的时间戳和体素格子的时间戳的偏差超过了一定的范围，就将点移动到另一个体素格子里面去，从而避免在体素进行整合的时候出现明显的偏差。但是论文并没有对这个体素格子的时间戳做说明，个人感觉应该就是进行了一个离群点的剔除，只不过这里离群的定义是依赖于时间戳，由于激光雷达的扫描方法，落入同一个体素格子的点的时间戳应该是大差不差的，用这个时间来做剔除，从而减小噪声点对体素化的负面影响。

3.2 里程计估计

里程计估计模块通过融合LiDAR和IMU测量值来补偿传感器的快速运动，并鲁棒地估计传感器状态。首先，基于IMU动力学的运动预测将点云数据转换到IMU帧，校正传感器运动引起的点云失真;然后，我们使用预先计算的邻近点来计算每个点的协方差。在点的偏移矫正完成之后，按照下图构建因子图。

…详情请参照古月居

经典文献阅读之--Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping（紧耦合3D激光雷达）相关推荐

【论文阅读】Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping
这篇论文提出了一个改进的地图生成方法,使用的传感器是激光雷达,所以这里也划分到激光SLAM的范畴下了.论文提出的框架的改进主要在两方面,一方面是使用了基于体素化的GICP匹配成本因子,采用这种方法可以 ...
货运服务网络设计：经典文献阅读笔记（3）复现Netplan
**货运服务网络设计:经典文献阅读笔记(2)**提到说要把Crainic T G(1984年)文献使用的模型复现一下,但是文章给出的通用框架还是太笼统,在尝试后决定使用Jacques Roy & ...
经典文献阅读之--Swin Transformer
0. 简介 Transfomer最近几年已经霸榜了各个领域,之前我们在<经典文献阅读之–Deformable DETR>这篇博客中对DETR这个系列进行了梳理,但是想着既然写了图像处理领域 ...
经典文献阅读之--NICE-SLAM（SLAM的神经隐含可扩展编码）
0. 简介对于深度学习而言,NeRF一定是最近两年最火的工作之一了,**NeRF(Neural Radiance Fields)**是最早在2020年ECCV会议上的Best Paper,其将隐式表 ...
经典文献阅读之--PL-SLAM(点线SLAM)
0. 简介之前作者基本都在围绕着特征点提取的路径在学习,最近看到了最近点云PCL推送的<Structure PLP-SLAM: Efficient Sparse Mapping and Loc ...
经典文献阅读之--PON
0. 简介作为Transformer在机器视觉领域的爆火,在自动驾驶领域目前很多工作都集中在前视转鸟瞰图的方法中,这里我们来讲2020年一篇经典的论文<Predicting Semantic ...
经典文献阅读之--SuMa++
0. 简介作为一名技术博主,最主要的还是需要不断地学习新的知识,而最好的学习就是不断地阅读新的文章,并不断地学习和总结前人的思路和方法.所以博主打算开一个新的系列来介绍.这里主要来介绍一下<S ...
经典文献阅读之--OV2SLAM(高速视觉slam)
0. 简介视觉里程计最近几年越来越受到学术界以及工业界的认可,以ORB和VINS为代表的视觉SLAM已经可以满足绝大多数场景,而OV2SLAM在其他VSLAM中脱颖而出,其实时性以及具体的回环性能在 ...
货运服务网络设计：经典文献阅读笔记（1）
服务网络设计 --(1984,Crainic,Multicommodity, Multimode Freight Transportation: A General Modeling and Algo ...

经典文献阅读之--Globally Consistent and Tightly Coupled 3D LiDAR Inertial Mapping（紧耦合3D激光雷达）