快速完整的基于点云闭环检测的激光SLAM系统

我原来总结过LOAM_Livox，这篇文章主要是解决LOAM在长时间运行的时累计误差的问题。本文提出的方法计算关键帧的2D直方图，局部地图patch，并使用2D直方图的归一化互相关(normalized cross-correlation)作为当前关键帧与地图中关键帧之间的相似性度量。这个方法快速且具有旋转不变性，鲁棒性高。

介绍

常用的闭环检测方法是词袋模型，把特的描述子聚类成单词，可以在词空间中计算观察值之间的相似度。不像视觉闭环的研究，基于雷达的闭环检测的工就很少，到现在也没有统一的开源的工程。主要的原因有雷达的价格太高，没有广泛被使用；基于点云的场景识别是很难的，不像图片中有丰富的纹理和颜色信息，点云里只有结构信息。
本文重要介绍闭环检测，地图配准和位姿图优化。

相关工作

• “Place recognition using keypoint voting in large 3d lidar datasets"直接利用点云中的3D特征点把他们描述成人工制作的3DGestalt描述子，然后利用投票的方法找到距离关键点最近的3D点。
• “Structure-based vision-laser matching”(2016 IROS)利用正态分布变(NDT)来描述3D点云的外观。利用NDT描述子的直方图计算两次扫描的相似度。
• “Segmatch： Segment based place recognition in 3d point clouds”通过匹配诸如建筑物，树等语义信息进行闭环检测。

但是到目前为止，针对于LOAM并没有开源的代码数据集。本文的主贡献是：

• 研究出来一个快速的闭环检测的方法来检测两个关键帧的相似度
• 把闭环检测，地图对齐，位姿优化集成到LOAM中。
• 通过在Github上开放数据集和源代码，我们为基于点云的闭环提供了可用的解决方案和范例。( https：//github。com/hku-mars/loam_livox

系统概述

系统的整体过程如下所示： 每一个传入的新帧和和全局的雷达地图配准(LOAM algorithm)，如果已接收到指定数量的帧（例如100帧），则会创建一个关键帧，该关键帧会形成一个小的本地地图补丁。通过LOAM将与新关键帧相对应的原始点云配准到全局地图中，以计算其2D直方图。将计算的2D直方图与数据库进行比较，该数据库包含由所有过去的关键帧组成的全局地图的2D直方图，以检测可能的闭环。同时，将新的关键帧2D直方图添加到数据库中以供下一个关键帧使用。一旦检测到闭环，就将关键帧与全局地图对齐，并执行位姿图优化以校正全局地图中的漂移。

地图和立方体

小胞体是有合适尺寸的小立方体(边长为Sx，Sy，Sz)，中心坐标为立方体中第一个点坐标。然后计算立方体中所有点的均值和协方差。需要注意的是，该单元格是3D空间的固定分区，并不断填充新点。为了加快均值和协方差的计算，我们可以通过小立方体存在的N个点推导出来加入来的点以后的新的均值和协方差。
地图是所有的保存在小胞体中的点的总和，地图点是用哈希表和八叉树表示的。利用哈希表可以通过立方体的中心快速找到胞体。通过八叉树可以快速找到给定范围内的所有的胞体。这两个策略对于地图对齐都很重要。
对于新来的胞体，通过其各个组成部分(中心坐标)的XOR操作来得到其哈希索引值。然后将计算出的哈希索引添加到地图H的哈希表中。由于该单元格是3D空间的固定分区，因此其中心位置是静态的，不需要更新哈希表中的现有条目（哈希表 虽然正在动态增长）。新插入的立方体根据中心坐标也被添加到八叉树地图中。
算法一：新帧配准
输入：第k帧的点云，当前的地图，，利用LOAM估计出来的相机位姿(R_k，T_k)
对于新帧中的每个点：

• 把每个点利用位姿转换到全局坐标系
• 利用公式1计算cell的中心
• 计算中心点的哈希值索引
• 如果这个哈希值不在哈希表
• 利用中心值创建一个新的cell
• 把地图的哈希索引的值插入到哈希表中
• 把中心值插入到地图的八叉树中
• 把这个点添加到cell中
• 更新cell的平均值
• 更新协方差矩阵

2D直方图的旋转不变性

快速闭检测主要思想是：我们使用类似于2D图的直方图粗略描述关键帧。2D直方图描述了关键帧中特征方向的Eulerangles的分布。

cell中特征的类型和方向

每个关键帧由一百次扫描的结果组成，对于每个cell，我们利用每个特征点和相关的特征的方向来确定他的形状。 第一步首先进行特征值分解，依据Appearance-based loop detection from 3d laser data using the normal distributions transform，根据分解出的特征值：

• 如果λ2远大于λ3，则将此cell视为平面特征，并将平面的法向量作为特征的方向；
• 如果cell不是平面而且λ1远大于λ2，则将此cell视为线特征，并将线的方向视为特征方向；

旋转不变性

为了保证每个特征都具有旋转不变性，我们利用一个额外的旋转矩阵旋转每个特征的方向，并以此保大多数特征都在x轴的方向。第二多的是在Y轴的方向上。因为平面特征更可靠，所以我们利用平特征的方向来确定旋转矩阵。

• 首先利用平面特征的方向向量来计算协方差矩阵
• 利用特征值分解协方差矩阵
• 利用特征向量得到旋转矩阵

算法二：计算关键帧的2D分布

• 输入关键帧F
• 输入2D线特征的分布和面特征的分布H_L，H_P
• 开始设置H_L,H_P为0，同时计算旋转矩

对于关键帧中的每个cell

• 如果cell是线特征
• 把特征×旋转矩阵
• 计算旋转后特征的pitch和yaw
• ?H_L[round(pitch/3)，round(yaw/3)]+=1
• 如果cell是面特征
• 把特征×旋转矩阵
• 计算旋转后特征的pitch和yaw
• H_P[round(pitch/3)，round(yaw/3)]+=1

对H_P和H_L进行滤波
每个关键帧包100次扫描结果，其包含了多个cell，每个关键帧由2个2D直方图组成：线分布和面分布。
利用旋转不变的cell特征的方向我们计算2D直方图：

• 选择X的分量为正的，计算特征分量的pitch和yaw
• 利用60*60的矩阵表示2D的直方图（每个pitch和yaw都有3度的分辨率）
• 利用pitch和yaw确定这个cell在矩中的位置
• 对每个2D直方图进行高斯滤波以提升鲁棒性

快速闭环检测

通过计算新帧的2D直方图和其他所有帧的相似度来检测闭环，这个关键帧和地图匹配然后地图利用位姿图优化的方进行更新。
（1）两帧相似度计算
使用2D直方图的归一化互相关来计算它们的相似性，论文中给出了相似度计算公式： 如果平面相似度计算值大于0.9或线段相似度计算值大于0.65，就可以认为检测出了闭环。

（2）地图对齐及优化
成功检测到闭环后，执行地图对齐以计算两个关键帧之间的相对位姿。地图对齐问题可以看作是目标点云和源点云之间的配准。由于LOAM算法中对线性形状和平面形状的像元进行了分类，因此我们使用边缘到边缘和平面到平面的特征来迭代求解相对姿势。 对齐后，如果边缘/平面特征上的点的平均距离足够接近边缘/平面特征（距离小于0。1m）则我们将这两张地图对齐。 （3） 位姿图优化 一旦两个关键帧对齐，执行位姿图优化。 我们使用Google ceres-solver实现图优化。优化位姿图后，我们通过重新计算包含的点，点的均值和协方差来更新整个地图中的所有像元。

参考文献：
[1] Lin J , Zhang F . A fast, complete, point cloud based loop closure for LiDAR odometry and mapping[J]. 2019.
[2] https://zhuanlan.zhihu.com/p/93600130