前往小程序,Get更优阅读体验!
立即前往
首页
学习
活动
专区
工具
TVP
发布
社区首页 >专栏 >快速完整的基于点云闭环检测的激光SLAM系统

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

作者头像
计算机视觉
发布2021-04-26 15:56:03
1.6K0
发布2021-04-26 15:56:03
举报

我原来总结过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

本文参与 腾讯云自媒体同步曝光计划,分享自微信公众号。
原始发表:2021-04-13,如有侵权请联系 cloudcommunity@tencent.com 删除

本文分享自 计算机视觉工坊 微信公众号,前往查看

如有侵权,请联系 cloudcommunity@tencent.com 删除。

本文参与 腾讯云自媒体同步曝光计划  ,欢迎热爱写作的你一起参与!

评论
登录后参与评论
0 条评论
热度
最新
推荐阅读
目录
  • 介绍
  • 相关工作
  • 系统概述
    • 地图和立方体
    • 2D直方图的旋转不变性
      • cell中特征的类型和方向
        • 旋转不变性
        • 快速闭环检测
        相关产品与服务
        图像处理
        图像处理基于腾讯云深度学习等人工智能技术,提供综合性的图像优化处理服务,包括图像质量评估、图像清晰度增强、图像智能裁剪等。
        领券
        问题归档专栏文章快讯文章归档关键词归档开发者手册归档开发者手册 Section 归档