CN111429344A - 基于感知哈希的激光slam闭环检测方法及*** - Google Patents
基于感知哈希的激光slam闭环检测方法及*** Download PDFInfo
- Publication number
- CN111429344A CN111429344A CN202010102282.1A CN202010102282A CN111429344A CN 111429344 A CN111429344 A CN 111429344A CN 202010102282 A CN202010102282 A CN 202010102282A CN 111429344 A CN111429344 A CN 111429344A
- Authority
- CN
- China
- Prior art keywords
- depth map
- rotation
- module
- radar
- calculating
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000001514 detection method Methods 0.000 title claims abstract description 27
- 238000000034 method Methods 0.000 claims abstract description 23
- 230000008569 process Effects 0.000 claims abstract description 16
- 238000005457 optimization Methods 0.000 claims abstract description 15
- 230000001131 transforming effect Effects 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 57
- 230000009466 transformation Effects 0.000 claims description 39
- 239000012141 concentrate Substances 0.000 claims description 6
- 239000004576 sand Substances 0.000 claims description 6
- 238000005549 size reduction Methods 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims 1
- 238000004364 calculation method Methods 0.000 description 7
- 238000006243 chemical reaction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/50—Depth or shape recovery
- G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T3/00—Geometric image transformations in the plane of the image
- G06T3/40—Scaling of whole images or parts thereof, e.g. expanding or contracting
- G06T3/4007—Scaling of whole images or parts thereof, e.g. expanding or contracting based on interpolation, e.g. bilinear interpolation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T3/00—Geometric image transformations in the plane of the image
- G06T3/60—Rotation of whole images or parts thereof
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/60—Analysis of geometric attributes
- G06T7/62—Analysis of geometric attributes of area, perimeter, diameter or volume
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10032—Satellite or aerial image; Remote sensing
- G06T2207/10044—Radar image
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20048—Transform domain processing
- G06T2207/20052—Discrete cosine transform [DCT]
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Mathematical Physics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Data Mining & Analysis (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Optics & Photonics (AREA)
- Algebra (AREA)
- Geometry (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
本发明提供了一种基于感知哈希的激光SLAM闭环检测方法及***,包括:激光扫描雷达每次旋转预设角度过程中扫描获得的点云,经过前端里程计和后端优化得到激光雷达位姿;根据获得的激光雷达位姿,构建每次旋转的深度图;根据每次旋转的深度图,计算深度图中心部分圆形区域的强度中心,确定深度图主方向;根据深度图主方向对深度图中各点坐标旋转变换,构建变换后的深度图;根据变换后的深度图感知哈希计算图片指纹,获得图片指纹;将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离,根据汉明距离计算相似度,当相似度最大值大于阈值时,则判断为闭环。本发明计算量小,适用于廉价的嵌入式设备,具有实时性,准确性高,抗噪声能力强的特点。
Description
技术领域
本发明涉及定位与导航领域,具体地,涉及一种基于感知哈希的激光SLAM闭环检测方法及***。
背景技术
SLAM(simultaneous localization and mapping),即时定位与地图构建。问题可以描述为:将一个机器人放入未知环境中的未知位置,是否有办法让机器人一边移动一边逐步描绘出此环境完全的地图,在递增地建立地图的同时,利用已建立的地图来同步刷新自身的位置。这是一个相辅相成不断迭代的过程,也是移动机器人自主完成作业任务需要解决的基本问题。目前依据使用的传感器不同,SLAM技术主要分为两大类:基于激光雷达的激光SLAM和基于摄像头的视觉SLAM。在测量平台移动的过程中,由于前端位姿估计和后端优化均基于部分相邻数据,之前产生的误差将不可避免地累计到下一个时刻,使得整个SLAM将会出现累积误差,长期估计的结果将不可靠。为消除这类误差,SLAM***中引入闭环检测模块。闭环检测,又称回环检测(Loop Closure Detection),主要解决位置估计随时间漂移的问题,通过识别测量平台曾经达到过的场景,从而在位姿优化中添加时隔更加久远的约束,使之得到更好的、全局一致的位姿估计。另一方面,由于闭环检测提供了当前数据与所有历史数据的关联,在跟踪丢失后,还可以利用闭环检测进行重定位。因此,闭环检测对整个SLAM***精度和鲁棒性有非常明显的提升。
传统的激光SLAM闭环检测算法主要有如下两种:
ICP算法,即迭代最近点(Iterative Closest Point):该算法是一种点集对点集配准方法,借助迭代优化的思想,通过旋转、平移使得两个点集之间累计的距离最小,从而实现相似部分点云的匹配。该方法有较为可靠的准确率,然而运算效率较低,在搜索对应点的过程中,计算量非常大,难以在廉价的嵌入式设备上实现实时运行。
词袋法(Bag of Words,BoW):该算法是一种基于特征点的匹配方法,首先从每次扫描的点云中提取部分特征点和特征描述,再将所有特征描述进行聚类,并组织成树的形式方便搜索构成词典,当两片点云中包含词典中一定量的相似点时,可以认为其存在相似性。然而该方仅仅基于图像局部特征,没有考虑特征点之间的相互关系,且特征点的稠密程度往往取决于工作环境,因而应用在激光SLAM上时准确度和可靠性较差。
发明内容
针对现有技术中的缺陷,本发明的目的是提供一种基于感知哈希的激光SLAM闭环检测方法及***。
根据本发明提供的一种基于感知哈希的激光SLAM闭环检测方法,包括:
步骤M1:激光扫描雷达每次旋转预设角度过程中扫描获得的点云,经过前端里程计和后端优化得到激光雷达位姿;
步骤M2:根据获得的激光雷达位姿,构建每次旋转的深度图;
步骤M3:根据每次旋转的深度图,计算深度图中心部分圆形区域的强度中心,确定深度图主方向;
步骤M4:根据深度图主方向对深度图中各点坐标旋转变换,构建变换后的深度图;
步骤M5:根据变换后的深度图感知哈希计算图片指纹,获得图片指纹;
步骤M6:将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离,根据汉明距离计算相似度,当相似度最大值大于阈值时,则判断为闭环。
优选地,所述步骤M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括:
对于第k次扫描,记第k次扫描起始时刻为tk,结束时刻为tk+1,两时刻间雷达位姿变换表达式为:
其中,tx,ty,tz分别表示雷达位姿沿雷达坐标系x,y,z轴的平移变换,θx,θy,θz分别表示旋转变换,符合右手定则;[]T表示转置;
使用线性插值法计算每次扫描时的雷达位姿:
所述步骤M2包括:
步骤M2.1:根据每次扫描的激光雷达位姿,依据欧氏变换,将当前扫描获得的点云投影到当前旋转起始时刻:
其中:
步骤M2.2:根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图,中点云各点坐标为三维坐标(x,y,z),转换为当前旋转所对应的深度图,即点云各点坐标(x,y)对应的深度D(x,y)=z,即tk坐标(x,y)各点对应的深度D(x,y)。
优选地,所述步骤M3包括:
计算深度图强度中心,所取的圆形区域表达式如下:
其中,(x,y)满足:x2+y2≤r2,则强度中心C定义为:C(xc,yc);
所述步骤M4包括:
步骤M4.1:为实现图像的旋转不变性,对深度图D中心部分进行坐标变换,得到变换后的深度图Dθ,对于变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)满足以下关系:
Dθ(x,y)=D(x′,y′) (12)
其中,Rθ为旋转矩阵,表达式如下:
对变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)使用双线性插值法计算;
优选地,所述步骤M5包括:
步骤M5.1:将深度图Dθ尺寸缩小为N×N,得到D′θ∈RN×N,R代表实数集合,RN×N代表N×N矩阵,且矩阵每个元素为实数;
步骤M5.2:对尺寸缩小后的深度图进行离散余弦变换,定义F∈RN×N为离散余弦变换结果,离散余弦变换结果F表达式如下:
其中,c(u)为补偿系数,c(v)表示补偿系数;如下所示:
优选地,所述步骤M6包括:
步骤M6.1:将hk与hj,j∈{1,2,…,k-1},分别计算汉明距离dkj,公式如下:
dkj=||hk-hj|| (16)
其中,hj表示每次旋转的深度图指纹;
步骤M6.2:根据汉明距离计算相似度skj,公式如下:
找到相似度的最大值ski:
当相似度的最大值ski大于阈值Ts,则判定当前雷达第k次旋转与第i次旋转时所在位置相同,即回环成立。
根据本发明提供的一种基于感知哈希的激光SLAM闭环检测***,包括:
模块M1:激光扫描雷达每次旋转预设角度过程中扫描获得的点云,经过前端里程计和后端优化得到激光雷达位姿;
模块M2:根据获得的激光雷达位姿,构建每次旋转的深度图;
模块M3:根据每次旋转的深度图,计算深度图中心部分圆形区域的强度中心,确定深度图主方向;
模块M4:根据深度图主方向对深度图中各点坐标旋转变换,构建变换后的深度图;
模块M5:根据变换后的深度图感知哈希计算图片指纹,获得图片指纹;
模块M6:将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离,根据汉明距离计算相似度,当相似度最大值大于阈值时,则判断为闭环。
优选地,所述模块M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括:
对于第k次扫描,记第k次扫描起始时刻为tk,结束时刻为tk+1,两时刻间雷达位姿变换表达式为:
其中,tx,ty,tz分别表示雷达位姿沿雷达坐标系x,y,z轴的平移变换,θx,θy,θz分别表示旋转变换,符合右手定则;[]T表示转置;
使用线性插值法计算每次扫描时的雷达位姿:
所述模块M2包括:
模块M2.1:根据每次扫描的激光雷达位姿,依据欧氏变换,将当前扫描获得的点云投影到当前旋转起始时刻:
其中:
模块M2.2:根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图,中点云各点坐标为三维坐标(x,y,z),转换为当前旋转所对应的深度图,即点云各点坐标(x,y)对应的深度D(x,y)=z,即tk坐标(x,y)各点对应的深度D(x,y)。
优选地,所述模块M3包括:
计算深度图强度中心,所取的圆形区域表达式如下:
其中,(x,y)满足:x2+y2≤r2,则强度中心C定义为:C(xc,yc);
所述模块M4包括:
模块M4.1:为实现图像的旋转不变性,对深度图D中心部分进行坐标变换,得到变换后的深度图Dθ,对于变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)满足以下关系:
Dθ(x,y)=D(x′,y′) (12)
其中,Rθ为旋转矩阵,表达式如下:
对变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x',y′)使用双线性插值法计算;
优选地,所述模块M5包括:
模块M5.1:将深度图Dθ尺寸缩小为N×N,得到D′θ∈RN×N,R代表实数集合,RN×N代表N×N矩阵,且矩阵每个元素为实数;
模块M5.2:对尺寸缩小后的深度图进行离散余弦变换,定义F∈RN×N为离散余弦变换结果,离散余弦变换结果F表达式如下:
其中,c(u)为补偿系数,c(v)表示补偿系数;如下所示:
优选地,所述模块M6包括:
模块M6.1:将hk与hj,j∈{1,2,…,k-1},分别计算汉明距离dkj,公式如下:
dkj=||hk-hj|| (16)
其中,hj表示每次旋转的深度图指纹;
模块M6.2:根据汉明距离计算相似度skj,公式如下:
找到相似度的最大值ski:
当相似度的最大值ski大于阈值Ts,则判定当前雷达第k次旋转与第i次旋转时所在位置相同,即回环成立。
与现有技术相比,本发明具有如下的有益效果:
1、本发明设计了一种轻量级的激光SLAM闭环检测方法,闭环检测问题转换为深度图匹配问题。首先将点云经过投影等变换后构造深度图,之后根据图像方向进行坐标旋转,再利用感知哈希算法通过离散余弦变换获取图片指纹,并通过计算图片指纹的汉明距离的方式完成闭环检测。值得注意的是,本发明计算量较小,便于在嵌入式设备上实时完成,且结果基于图像整体特征,准确率较高,抗噪声能力强的特点。另外,由于图片指纹基于与平均值的大小比较,因而本算法对于局部噪声不敏感,具有较好的稳定性。
2、本发明创造性的将强度中心与感知哈希相结合,通过计算图片的强度中心坐标确定图片方向,进而对图像进行旋转坐标变换,确保匹配过程中的旋转不变性,可以大幅提高匹配的准确率。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述,本发明的其它特征、目的和优点将会变得更明显:
图1为闭环检测流程;
图2为深度图构建流程;
图3为坐标旋转变换流程;
图4为感知哈希匹配流程;
图5为坐标旋转示意图;
图6为双线性插值法示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明,但不以任何形式限制本发明。应当指出的是,对本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变化和改进。这些都属于本发明的保护范围。
本发明提供了一种基于感知哈希的激光SLAM闭环检测方法,该方法将SLAM闭环检测的问题转化为深度图相似图片检索问题,特点是首先确定强度中心的坐标确定图片方向,并进行坐标转换以确保旋转不变性,之后使用感知哈希对深度图进行编码,进而在哈希编码的基础上进行相似度计算,实现相似深度图的检索进而完成SLAM闭环检测。
根据本发明提供的一种基于感知哈希的激光SLAM闭环检测方法,包括:
步骤M1:3D激光扫描雷达每次旋转180°过程中扫描获得的点云,经过前端里程计和后端优化得到激光雷达位姿;
具体地,所述步骤M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括:
对于第k次扫描,记第k次扫描起始时刻为tk,结束时刻为tk+1,两时刻间雷达位姿变换表达式为:
其中,tx,ty,tz分别表示雷达位姿沿雷达坐标系x,y,z轴的平移变换,θx,θy,θz分别表示旋转变换,符合右手定则;[]T表示转置;
使用线性插值法计算每次扫描时的雷达位姿:
步骤M2:根据获得的激光雷达位姿,获得当前旋转的点云信息,构建每次旋转的深度图;
所述步骤M2包括:
步骤M2.1:根据每次扫描的激光雷达位姿,依据欧氏变换,将当前扫描获得的点云投影到当前旋转起始时刻:
其中:
步骤M2.2:根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图,中点云各点坐标为三维坐标(x,y,z),转换为当前旋转所对应的深度图,即点云各点坐标(x,y)对应的深度D(x,y)=z,即tk坐标(x,y)各点对应的深度D(x,y)。
步骤M3:根据每次旋转的深度图,计算深度图中心部分圆形区域的强度中心,确定深度图主方向;
具体地,所述步骤M3包括:
计算深度图强度中心,所取的圆形区域表达式如下:
其中,(x,y)满足:x2+y2≤r2,则强度中心C定义为:C(xc,yc);
步骤M4:根据深度图主方向对深度图中各点坐标旋转变换,构建变换后的深度图;
所述步骤M4包括:
步骤M4.1:为实现图像的旋转不变性,对深度图D中心部分进行坐标变换,得到变换后的深度图Dθ,对于变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)满足以下关系:
Dθ(x,y)=D(x′,y′) (12)
其中,Rθ为旋转矩阵,表达式如下:
由于以上计算得到的x′,y′可能不为整数,对变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)使用双线性插值法计算;
其中,dx1,dx2,dy1,dy2如下定义:
步骤M5:根据变换后的深度图感知哈希计算图片指纹,获得图片指纹;
具体地,所述步骤M5包括:
步骤M5.1:将深度图Dθ尺寸缩小为N×N,这里的N为4的倍数,得到D′θ∈RN×N,R代表实数集合,RN×N代表N×N矩阵,且矩阵每个元素为实数;
步骤M5.2:对尺寸缩小后的深度图进行离散余弦变换,定义F∈RN×N为离散余弦变换结果,离散余弦变换结果F表达式如下:
其中,c(u)为补偿系数,c(v)表示补偿系数;如下所示:
步骤M6:将获得图片指纹与每次依次按照步骤M3、M4、M5得到每次旋转的深度图指纹依次计算汉明距离,根据汉明距离计算相似度,当相似度最大值大于阈值时,则判断为闭环。
具体地,所述步骤M6包括:
步骤M6.1:将hk与hj,j∈{1,2,…,k-1},分别计算汉明距离dkj,公式如下:
dkj=||hk-hj|| (16)
其中,hj表示每次旋转的深度图指纹;
步骤M6.2:根据汉明距离计算相似度skj,公式如下:
找到相似度的最大值ski:
当相似度的最大值ski大于阈值Ts,则判定当前雷达第k次旋转与第i次旋转时所在位置相同,即回环成立。
当缩小后尺寸N=32时,则相似度阈值Ts=90.6度。
根据本发明提供的一种基于感知哈希的激光SLAM闭环检测***,包括:
模块M1:3D激光扫描雷达每次旋转180°过程中扫描获得的点云,经过前端里程计和后端优化得到激光雷达位姿;
具体地,所述模块M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括:
对于第k次扫描,记第k次扫描起始时刻为tk,结束时刻为tk+1,两时刻间雷达位姿变换表达式为:
其中,tx,ty,tz分别表示雷达位姿沿雷达坐标系x,y,z轴的平移变换,θx,θy,θz分别表示旋转变换,符合右手定则;[]T表示转置;
使用线性插值法计算每次扫描时的雷达位姿:
模块M2:根据获得的激光雷达位姿,获得当前旋转的点云信息,构建每次旋转的深度图;
所述模块M2包括:
模块M2.1:根据每次扫描的激光雷达位姿,依据欧氏变换,将当前扫描获得的点云投影到当前旋转起始时刻:
其中:
模块M2.2:根据当前扫描获得的点云投影到当前旋转起始时刻的点云坐标建立当前旋转所对应的深度图,中点云各点坐标为三维坐标(x,y,z),转换为当前旋转所对应的深度图,即点云各点坐标(x,y)对应的深度D(x,y)=z,即tk坐标(x,y)各点对应的深度D(x,y)。
模块M3:根据每次旋转的深度图,计算深度图中心部分圆形区域的强度中心,确定深度图主方向;
具体地,所述模块M3包括:
计算深度图强度中心,所取的圆形区域表达式如下:
其中,(x,y)满足:x2+y2≤r2,则强度中心C定义为:C(xc,yc);
模块M4:根据深度图主方向对深度图中各点坐标旋转变换,构建变换后的深度图;
所述模块M4包括:
模块M4.1:为实现图像的旋转不变性,对深度图D中心部分进行坐标变换,得到变换后的深度图Dθ,对于变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)满足以下关系:
Dθ(x,y)=D(x′,y') (12)
其中,Rθ为旋转矩阵,表达式如下:
由于以上计算得到的x′,y′可能不为整数,对变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)使用双线性插值法计算;
其中,dx1,dx2,dy1,dy2如下定义:
模块M5:根据变换后的深度图感知哈希计算图片指纹,获得图片指纹;
具体地,所述模块M5包括:
模块M5.1:将深度图Dθ尺寸缩小为N×N,这里的N为4的倍数,得到D′θ∈RN×N,R代表实数集合,RN×N代表N×N矩阵,且矩阵每个元素为实数;
模块M5.2:对尺寸缩小后的深度图进行离散余弦变换,定义F∈RN×N为离散余弦变换结果,离散余弦变换结果F表达式如下:
其中,c(u)为补偿系数,c(v)表示补偿系数;如下所示:
模块M6:将获得图片指纹与每次依次按照步骤M3、M4、M5得到每次旋转的深度图指纹依次计算汉明距离,根据汉明距离计算相似度,当相似度最大值大于阈值时,则判断为闭环。
具体地,所述模块M6包括:
模块M6.1:将hk与hj,j∈{1,2,…,k-1},分别计算汉明距离dkj,公式如下:
dkj=||hk-hj|| (16)
其中,hj表示每次旋转的深度图指纹;
模块M6.2:根据汉明距离计算相似度skj,公式如下:
找到相似度的最大值ski:
当相似度的最大值ski大于阈值Ts,则判定当前雷达第k次旋转与第i次旋转时所在位置相同,即回环成立。
当缩小后尺寸N=32时,则相似度阈值Ts=90.6度。
以下优选例对本发明作进一步详细说明:
在优选例中,所述步骤M1中,雷达扫描器每次可以扫描一个平面,视角为180°。为实现3D扫描,扫描器由马达驱动,以扫描器水平时扫描平面为参照,由-90°旋转至90°,扫描器每次扫描后,点云中各点坐标可以直接由传感器获取。
在优选例中,所述步骤M1和M2中,雷达坐标系{L}为一个3D直角坐标系,其原点位于雷达的几何中心,x轴指向雷达左侧,y轴指向上方,z轴指向雷达前方,在雷达第k次旋转时,其雷达坐标系记为{Lk},其中点i的坐标记为特别说明的是,激光SLAM运行过程中,前端里程计和后端优化将会实时计算雷达每次旋转位姿变换在闭环检测过程中可以直接获取而无需额外计算。
在优选例中,所述步骤M2.2中,具体实现如下:
将点云数据转为深度图,即将平面深度图上的每个像素点对应空间中某一小块点云。例如将角分辨率定义为0.25°,意味着深度图临近的像素点对应的点云中的点与雷达坐标系原点连线夹角相差0.25°。
在优选例中,利用pcl库中的range_image即可实现点云到深度图的转换,设置参数角度分辨率为0.25°,设置水平视角和垂直视角均为180°,即可将传感器前方点云构建为720×720的深度图。
在优选例中,步骤M3.1选取的中心部分为圆心与深度图中心重合,半径r=360像素的圆。
在优选例中,步骤M3.1和步骤M3.2中计算图像强度中心C的坐标以及方向角θ仅涉及到上述圆中的各点,与边缘部分无关。
在优选例中,所述步骤M4.1中,包含如下步骤:
(1)确定变换后的深度图Dθ的坐标范围,如下所示:
即变换后的深度图Dθ大小为509×509,中心与原深度图D中心重合。
(2)如步骤M4.1所述计算变换后的深度图各点取值Dθ(x,y),参见附图5:
Dθ(x,y)=D(x′,y′)=D(x·cosθ-y·sihθ,x·sinθ+y·cosθ)
(3)对于非整数的x′,y′,如步骤M4.1所述使用双线性插值法计算,见附图6。
在优选例中,所述步骤M5.1中,变换后的深度图尺度需由509×509,压缩为32×32,记为D′θ,图片缩放通过双线性插值法实现,在实际操作中,可以使用OpenCV库中的resize函数完成。
在优选例中,所述步骤M5.2中,为便于计算,应将离散余弦变换的计算转换为矩阵形式,如下所示:
F=AD′θAT
其中,F∈R32×32为离散余弦变换后的结果,D′θ∈R32×32为变换后的深度图,c(i)为步骤S3.2所述补偿系数,矩阵A∈R32×32,其中各项元素如上所述。实际操作中可以使用numpy库完成以上运算,加快运算速度。
在优选例中,所述步骤M5.3中,只需保留矩阵F左上角8*8的部分,记作F′∈R8×8。
在优选例中,所述步骤M5.4中,将F′中大于平均值的元素均设为“1”,小于平均值的元素均设为“0”,可以得到一个8*8的矩阵,每个元素均为“0”或“1”。
在优选例中,所述步骤M5.5中,将上一步所得矩阵按列相接,得到hk∈R64,如下所示:
在优选例中,所述步骤M6中,如下计算当前深度图指纹向量hk与已有深度图指纹hj,j∈{1,2...,k-1}相似度:
设定相似度阈值Ts=90.6度,若相似度大于这一阈值则判定回环成立。
本领域技术人员知道,除了以纯计算机可读程序代码方式实现本发明提供的***、装置及其各个模块以外,完全可以通过将方法步骤进行逻辑编程来使得本发明提供的***、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以,本发明提供的***、装置及其各个模块可以被认为是一种硬件部件,而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构;也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是,本发明并不局限于上述特定实施方式,本领域技术人员可以在权利要求的范围内做出各种变化或修改,这并不影响本发明的实质内容。在不冲突的情况下,本申请的实施例和实施例中的特征可以任意相互组合。
Claims (10)
1.一种基于感知哈希的激光SLAM闭环检测方法,其特征在于,包括:
步骤M1:激光扫描雷达每次旋转预设角度过程中扫描获得的点云,经过前端里程计和后端优化得到激光雷达位姿;
步骤M2:根据获得的激光雷达位姿,构建每次旋转的深度图;
步骤M3:根据每次旋转的深度图,计算深度图中心部分圆形区域的强度中心,确定深度图主方向;
步骤M4:根据深度图主方向对深度图中各点坐标旋转变换,构建变换后的深度图;
步骤M5:根据变换后的深度图感知哈希计算图片指纹,获得图片指纹;
步骤M6:将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离,根据汉明距离计算相似度,当相似度最大值大于阈值时,则判断为闭环。
2.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法,其特征在于,所述步骤M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括:
对于第k次扫描,记第k次扫描起始时刻为tk,结束时刻为tk+1,两时刻间雷达位姿变换表达式为:
其中,tx,ty,tz分别表示雷达位姿沿雷达坐标系x,y,z轴的平移变换,θx,θy,θz分别表示旋转变换,符合右手定则;[]T表示转置;
使用线性插值法计算每次扫描时的雷达位姿:
所述步骤M2包括:
步骤M2.1:根据每次扫描的激光雷达位姿,依据欧氏变换,将当前扫描获得的点云投影到当前旋转起始时刻:
其中:
3.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法,其特征在于,所述步骤M3包括:
计算深度图强度中心,所取的圆形区域表达式如下:
其中,(x,y)满足:x2+y2≤r2,则强度中心C定义为:C(xc,yc);
所述步骤M4包括:
步骤M4.1:为实现图像的旋转不变性,对深度图D中心部分进行坐标变换,得到变换后的深度图Dθ,对于变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)满足以下关系:
Dθ(x,y)=D(x′,y′) (12)
其中,Rθ为旋转矩阵,表达式如下:
对变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)使用双线性插值法计算。
4.根据权利要求1所述的一种基于感知哈希的激光SLAM闭环检测方法,其特征在于,所述步骤M5包括:
步骤M5.1:将深度图Dθ尺寸缩小为N×N,得到D′0∈RN×N,R代表实数集合,RN×N代表N×N矩阵,且矩阵每个元素为实数;
步骤M5.2:对尺寸缩小后的深度图进行离散余弦变换,定义F∈RN×N为离散余弦变换结果,离散余弦变换结果F表达式如下:
其中,c(u)为补偿系数,c(v)表示补偿系数;如下所示:
6.一种基于感知哈希的激光SLAM闭环检测***,其特征在于,包括:
模块M1:激光扫描雷达每次旋转预设角度过程中扫描获得的点云,经过前端里程计和后端优化得到激光雷达位姿;
模块M2:根据获得的激光雷达位姿,构建每次旋转的深度图;
模块M3:根据每次旋转的深度图,计算深度图中心部分圆形区域的强度中心,确定深度图主方向;
模块M4:根据深度图主方向对深度图中各点坐标旋转变换,构建变换后的深度图;
模块M5:根据变换后的深度图感知哈希计算图片指纹,获得图片指纹;
模块M6:将获得图片指纹与每次旋转的深度图指纹依次计算汉明距离,根据汉明距离计算相似度,当相似度最大值大于阈值时,则判断为闭环。
7.根据权利要求6所述的一种基于感知哈希的激光SLAM闭环检测***,其特征在于,所述模块M1中扫描获得的点云经过前端里程计和后端优化得到的激光雷达位姿包括:
对于第k次扫描,记第k次扫描起始时刻为tk,结束时刻为tk+1,两时刻间雷达位姿变换表达式为:
其中,tx,ty,tz分别表示雷达位姿沿雷达坐标系x,y,z轴的平移变换,θx,θy,θz分别表示旋转变换,符合右手定则;[]T表示转置;
使用线性插值法计算每次扫描时的雷达位姿:
所述模块M2包括:
模块M2.1:根据每次扫描的激光雷达位姿,依据欧氏变换,将当前扫描获得的点云投影到当前旋转起始时刻:
其中:
8.根据权利要求6所述的一种基于感知哈希的激光SLAM闭环检测***,其特征在于,所述模块M3包括:
计算深度图强度中心,所取的圆形区域表达式如下:
其中,(x,y)满足:x2+y2≤r2,则强度中心C定义为:C(xc,yc);
所述模块M4包括:
模块M4.1:为实现图像的旋转不变性,对深度图D中心部分进行坐标变换,得到变换后的深度图Dθ,对于变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)满足以下关系:
Dθ(x,y)=D(x′,y′) (12)
其中,Rθ为旋转矩阵,表达式如下:
对变换后的深度图Dθ中的坐标Dθ(x,y)在深度图D中对应的坐标D(x′,y′)使用双线性插值法计算。
9.根据权利要求6所述的一种基于感知哈希的激光SLAM闭环检测***,其特征在于,所述模块M5包括:
模块M5.1:将深度图Dθ尺寸缩小为N×N,得到D′0∈RN×N,R代表实数集合,RN×N代表N×N矩阵,且矩阵每个元素为实数;
模块M5.2:对尺寸缩小后的深度图进行离散余弦变换,定义F∈RN×N为离散余弦变换结果,离散余弦变换结果F表达式如下:
其中,c(u)为补偿系数,c(v)表示补偿系数;如下所示:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010102282.1A CN111429344B (zh) | 2020-02-19 | 2020-02-19 | 基于感知哈希的激光slam闭环检测方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010102282.1A CN111429344B (zh) | 2020-02-19 | 2020-02-19 | 基于感知哈希的激光slam闭环检测方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111429344A true CN111429344A (zh) | 2020-07-17 |
CN111429344B CN111429344B (zh) | 2022-04-26 |
Family
ID=71547184
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010102282.1A Active CN111429344B (zh) | 2020-02-19 | 2020-02-19 | 基于感知哈希的激光slam闭环检测方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111429344B (zh) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111862162A (zh) * | 2020-07-31 | 2020-10-30 | 湖北亿咖通科技有限公司 | 回环检测方法及***、可读存储介质、电子设备 |
CN112053390A (zh) * | 2020-09-04 | 2020-12-08 | 上海懒书智能科技有限公司 | 一种基于点云变换匹配的定位方法 |
CN112230243A (zh) * | 2020-10-28 | 2021-01-15 | 西南科技大学 | 一种移动机器人室内地图构建方法 |
CN113470113A (zh) * | 2021-08-13 | 2021-10-01 | 西南科技大学 | 一种融合brief特征匹配与icp点云配准的零部件姿态估计方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107403163A (zh) * | 2017-07-31 | 2017-11-28 | 武汉大学 | 一种基于深度学习的激光slam闭环检测方法 |
CN108008394A (zh) * | 2017-11-28 | 2018-05-08 | 安徽工程大学 | 仿生声纳二维经历图绘制方法、存储介质及设备 |
CN110120093A (zh) * | 2019-03-25 | 2019-08-13 | 深圳大学 | 一种多元特征混合优化的rgb-d室内三维测图方法及*** |
CN110163968A (zh) * | 2019-05-28 | 2019-08-23 | 山东大学 | Rgbd相机大型三维场景构建方法及*** |
WO2020030951A1 (en) * | 2018-08-08 | 2020-02-13 | Abyssal S.A. | System and method of operation for remotely operated vehicles for simultaneous localization and mapping |
-
2020
- 2020-02-19 CN CN202010102282.1A patent/CN111429344B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107403163A (zh) * | 2017-07-31 | 2017-11-28 | 武汉大学 | 一种基于深度学习的激光slam闭环检测方法 |
CN108008394A (zh) * | 2017-11-28 | 2018-05-08 | 安徽工程大学 | 仿生声纳二维经历图绘制方法、存储介质及设备 |
WO2020030951A1 (en) * | 2018-08-08 | 2020-02-13 | Abyssal S.A. | System and method of operation for remotely operated vehicles for simultaneous localization and mapping |
CN110120093A (zh) * | 2019-03-25 | 2019-08-13 | 深圳大学 | 一种多元特征混合优化的rgb-d室内三维测图方法及*** |
CN110163968A (zh) * | 2019-05-28 | 2019-08-23 | 山东大学 | Rgbd相机大型三维场景构建方法及*** |
Non-Patent Citations (2)
Title |
---|
CHUNXU CHEN等: "Trajectory Optimization of LiDAR SLAM Based on Local Pose Graph", 《CSNC 2019》 * |
HENG ZHANG等: "Loop Closing Detection in RGB-D SLAM Combining Appearance and Geometric Constraints", 《SENSORS》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111862162A (zh) * | 2020-07-31 | 2020-10-30 | 湖北亿咖通科技有限公司 | 回环检测方法及***、可读存储介质、电子设备 |
CN112053390A (zh) * | 2020-09-04 | 2020-12-08 | 上海懒书智能科技有限公司 | 一种基于点云变换匹配的定位方法 |
CN112053390B (zh) * | 2020-09-04 | 2023-12-22 | 上海懒书智能科技有限公司 | 一种基于点云变换匹配的定位方法 |
CN112230243A (zh) * | 2020-10-28 | 2021-01-15 | 西南科技大学 | 一种移动机器人室内地图构建方法 |
CN113470113A (zh) * | 2021-08-13 | 2021-10-01 | 西南科技大学 | 一种融合brief特征匹配与icp点云配准的零部件姿态估计方法 |
CN113470113B (zh) * | 2021-08-13 | 2023-07-21 | 西南科技大学 | 一种融合brief特征匹配与icp点云配准的零部件姿态估计方法 |
Also Published As
Publication number | Publication date |
---|---|
CN111429344B (zh) | 2022-04-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Li et al. | Point cloud registration based on one-point ransac and scale-annealing biweight estimation | |
CN111429344B (zh) | 基于感知哈希的激光slam闭环检测方法及*** | |
EP2048599B1 (en) | System and method for 3D object recognition | |
CN111750820B (zh) | 影像定位方法及其*** | |
Khoshelham | Extending generalized hough transform to detect 3d objects in laser range data | |
CN108122256B (zh) | 一种逼近状态下旋转目标位姿测量的方法 | |
Azad et al. | Stereo-based 6d object localization for grasping with humanoid robot systems | |
CN110966991A (zh) | 一种无控制点下的单幅无人机影像定位方法 | |
CN102722697B (zh) | 一种无人飞行器视觉自主导引着陆的目标跟踪方法 | |
Houshiar et al. | A study of projections for key point based registration of panoramic terrestrial 3D laser scan | |
Li et al. | Automatic targetless LiDAR–camera calibration: a survey | |
EP3404583A1 (en) | System and method for model adaptation | |
Bu et al. | Semi-direct tracking and mapping with RGB-D camera for MAV | |
CN111798453A (zh) | 用于无人驾驶辅助定位的点云配准方法及其*** | |
Ma et al. | Visual homing via guided locality preserving matching | |
CN114529681A (zh) | 一种手持双相机建筑物温度场三维模型构建方法和*** | |
WO2008032375A1 (fr) | Dispositif et procédé de correction d'image, et programme informatique | |
Pandey et al. | Alignment of 3D point clouds with a dominant ground plane | |
KR101733288B1 (ko) | 방향정보를 이용한 객체 검출기 생성 방법, 이를 이용한 객체 검출 장치 및 방법 | |
Kang et al. | A robust image matching method based on optimized BaySAC | |
Tornow et al. | A multi-agent mobile robot system with environment perception and HMI capabilities | |
Zang et al. | Camera localization by CAD model matching | |
Lakshmi et al. | Study of feature based image registration algorithms for navigation of unmanned aerial vehicles | |
Jackson et al. | Adaptive registration of very large images | |
Hu et al. | Accurate fiducial mapping for pose estimation using manifold optimization |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |