快速完整的基于点云闭环检测的激光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

备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区

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

下载1

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

下载2

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

下载3

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

重磅！3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信，可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群，旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群，目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注：研究方向+学校/公司+昵称，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导。与此同时，星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息，打造成集技术与就业为一体的铁杆粉丝聚集区，近3000星球成员为创造更好的AI世界共同进步，知识星球入口：

学习3D视觉核心技术，扫描查看介绍，3天内无条件退款

 圈里有高质量教程资料、可答疑解惑、助你高效解决问题

觉得有用，麻烦给个赞和在看~