我原来总结过LOAM_Livox,这篇文章主要是解决LOAM在长时间运行的时累计误差的问题。本文提出的方法计算关键帧的2D直方图,局部地图patch,并使用2D直方图的归一化互相关(normalized cross-correlation)作为当前关键帧与地图中关键帧之间的相似性度量。这个方法快速且具有旋转不变性,鲁棒性高。
常用的闭环检测方法是词袋模型,把特的描述子聚类成单词,可以在词空间中计算观察值之间的相似度。不像视觉闭环的研究,基于雷达的闭环检测的工就很少,到现在也没有统一的开源的工程。主要的原因有雷达的价格太高,没有广泛被使用;基于点云的场景识别是很难的,不像图片中有丰富的纹理和颜色信息,点云里只有结构信息。本文重要介绍闭环检测,地图配准和位姿图优化。
但是到目前为止,针对于LOAM并没有开源的代码数据集。本文的主贡献是:
系统的整体过程如下所示:每一个传入的新帧和和全局的雷达地图配准(LOAM algorithm),如果已接收到指定数量的帧(例如100帧),则会创建一个关键帧,该关键帧会形成一个小的本地地图补丁。通过LOAM将与新关键帧相对应的原始点云配准到全局地图中,以计算其2D直方图。将计算的2D直方图与数据库进行比较,该数据库包含由所有过去的关键帧组成的全局地图的2D直方图,以检测可能的闭环。同时,将新的关键帧2D直方图添加到数据库中以供下一个关键帧使用。一旦检测到闭环,就将关键帧与全局地图对齐,并执行位姿图优化以校正全局地图中的漂移。
小胞体是有合适尺寸的小立方体(边长为Sx,Sy,Sz),中心坐标为立方体中第一个点坐标。然后计算立方体中所有点的均值和协方差。需要注意的是,该单元格是3D空间的固定分区,并不断填充新点。为了加快均值和协方差的计算,我们可以通过小立方体存在的N个点推导出来加入来的点以后的新的均值和协方差。地图是所有的保存在小胞体中的点的总和,地图点是用哈希表和八叉树表示的。利用哈希表可以通过立方体的中心快速找到胞体。通过八叉树可以快速找到给定范围内的所有的胞体。这两个策略对于地图对齐都很重要。对于新来的胞体,通过其各个组成部分(中心坐标)的XOR操作来得到其哈希索引值。然后将计算出的哈希索引添加到地图H的哈希表中。由于该单元格是3D空间的固定分区,因此其中心位置是静态的,不需要更新哈希表中的现有条目(哈希表 虽然正在动态增长)。新插入的立方体根据中心坐标也被添加到八叉树地图中。
算法一:新帧配准
输入:第k帧的点云,当前的地图,利用LOAM估计出来的相机位姿(R_k,T_k)对于新帧中的每个点:
快速闭检测主要思想是:我们使用类似于2D图的直方图粗略描述关键帧。2D直方图描述了关键帧中特征方向的Eulerangles的分布。
每个关键帧由一百次扫描的结果组成,对于每个cell,我们利用每个特征点和相关的特征的方向来确定他的形状。第一步首先进行特征值分解,依据Appearance-based loop detection from 3d laser data using the normal distributions transform,根据分解出的特征值:
为了保证每个特征都具有旋转不变性,我们利用一个额外的旋转矩阵旋转每个特征的方向,并以此保大多数特征都在x轴的方向。第二多的是在Y轴的方向上。因为平面特征更可靠,所以我们利用平特征的方向来确定旋转矩阵。
算法二:计算关键帧的2D分布
H_L,H_P
H_L,H_P
为0,同时计算旋转矩对于关键帧中的每个cell
对H_P和H_L进行滤波每个关键帧包100次扫描结果,其包含了多个cell,每个关键帧由2个2D直方图组成:线分布和面分布。利用旋转不变的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