CN115047487A - 基于点云强度与高度信息的回环检测方法及设备 - Google Patents

基于点云强度与高度信息的回环检测方法及设备 Download PDF

Info

Publication number
CN115047487A
CN115047487A CN202210235580.7A CN202210235580A CN115047487A CN 115047487 A CN115047487 A CN 115047487A CN 202210235580 A CN202210235580 A CN 202210235580A CN 115047487 A CN115047487 A CN 115047487A
Authority
CN
China
Prior art keywords
point cloud
matrix
point
similarity
column vectors
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.)
Pending
Application number
CN202210235580.7A
Other languages
English (en)
Inventor
蒋林
廖雅曼
潘锋
王振宇
颜俊杰
胡雨欣
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhan University of Science and Engineering WUSE
Original Assignee
Wuhan University of Science and Engineering WUSE
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Wuhan University of Science and Engineering WUSE filed Critical Wuhan University of Science and Engineering WUSE
Priority to CN202210235580.7A priority Critical patent/CN115047487A/zh
Publication of CN115047487A publication Critical patent/CN115047487A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
    • G01S7/497—Means for monitoring or calibrating
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/005—Tree description, e.g. octree, quadtree
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
    • G06T17/05—Geographic models
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/003—Navigation within 3D models or images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Software Systems (AREA)
  • Theoretical Computer Science (AREA)
  • Remote Sensing (AREA)
  • Geometry (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Graphics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Hardware Design (AREA)
  • General Engineering & Computer Science (AREA)
  • Electromagnetism (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明提供了一种基于点云强度与高度信息的回环检测方法及设备。所述方法包括：步骤1：将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下；步骤2：对同一帧点云的几何与强度信息进行编码来构建点云的全局描述符；步骤3：将全局描述符中的两层矩阵转换为两个列向量，取矩阵每行所有元素的平均值作为列向量每行的值；步骤4：用两个KD树分别来保存所有历史点云的列向量；通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的若干帧历史点云；步骤5：计算当前点云和所有候选点云的相似度，选出其中最大的相似度，若相似度大于阈值，则回环检测成功。本发明减少为克服旋转不变性问题产生的计算。

Description

基于点云强度与高度信息的回环检测方法及设备
技术领域
本发明实施例涉及机器人平台建图技术领域，尤其涉及一种基于点云强度与高度信息的回环检测方法及设备。
背景技术
回环检测算法用于判断机器人是否回到了曾经抵达过的位置，进而得到约束来消除机器人在SLAM过程(即同时定位与地图创建)中产生的不可避免的累计误差。正确的回环检测能帮助SLAM系统提高所构建地图的全局一致性，错误的回环检测结果会影响全局地图的构建。因此，开发一种基于点云强度与高度信息的回环检测方法及设备，可以在机器人的SLAM过程中提供准确的回环检测结果，就成为业界亟待解决的技术问题。
发明内容
针对现有技术存在的上述问题，本发明实施例提供了一种基于点云强度与高度信息的回环检测方法及设备。
第一方面，本发明的实施例提供了一种基于点云强度与高度信息的回环检测方法，包括：步骤1：采用SLAM得到的点云位姿，将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下；步骤2：通过点云高度信息编码和点云强度信息编码，对同一帧点云的几何与强度信息进行编码来构建点云的全局描述符；步骤3：将全局描述符中的两层矩阵转换为两个列向量，取矩阵每行所有元素的平均值作为列向量每行的值；步骤4：用两个KD树分别来保存所有历史点云的列向量；通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的若干帧历史点云；步骤5：计算当前点云和所有候选点云的相似度，选出其中最大的相似度，若相似度大于阈值，则回环检测成功，进行回环修正；否则，回环检测失败，进入下一帧。
在上述方法实施例内容的基础上，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下，包括：通过旋转矩阵将当前帧点云姿态从雷达坐标系下变换为全局坐标系下的姿态。
在上述方法实施例内容的基础上，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤2中的全局描述符的第一层是通过将点云投影到二维平面进行编码得到对应的矩阵得到的，包括：对于一帧激光点云数据，定义一距离阈值Dmax，取点云中距离点云中心小于阈值Dmax的点，得到点云P；通过半径将点云P在xy平面上均匀的划分为若干扇形区域，扇形数量为Nr；用相等距离同心圆将所有扇形划分为若干区域，设同心圆相距为Dc，则每个扇形被划分为Nc＝Dmax/Dc个区域；将点云划分为Nc×Nr个区域，用Emn表示所有区域，则m和n分别满足：
Figure BDA0003541999030000021
N为自然数集，划分得到的所有区域和大小为Nc×Nr的矩阵的元素一一对应；取每个区域中最高点的高度值为矩阵中对应元素的值，得到高度矩阵，高度矩阵每个元素的取值可以表示为：
Figure BDA0003541999030000022
其中Z(pn)函数为点pn在Z轴上的取值，p(Emn)表示的是区域Emn内的所有点，如果Emn区域内没有点，则对应矩阵元素取值为0。
在上述方法实施例内容的基础上，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤2中的全局描述符的第二层采用点云的强度信息构建，包括：将点云P中的点转换到球面坐标系；再将所有的点对应到大小为h×w的矩阵MI中，点云中每个点pn＝(x,y,z)和矩阵MI元素的映射关系可以表示为：
Figure BDA0003541999030000023
其中，r为点到点云中心的距离，f表示的是激光雷达垂直方向上的视角，w和h代表对应矩阵的列数与行数，u和v分别代表点所对应元素在矩阵中的行与列；将点云P中点的强度值赋予给矩阵MI所对应元素。
在上述方法实施例内容的基础上，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤3中的将全局描述符中的两层矩阵转换为两个列向量，包括：将全局描述符中的两层矩阵MH、MI转换为两个列向量VH、VI，取矩阵每行所有元素的平均值作为列向量每行的值，包括：
Figure BDA0003541999030000031
Figure BDA0003541999030000032
其中，i为矩阵的第i行，即列向量的第i行,n为矩阵的第n列。
在上述方法实施例内容的基础上，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤4中的所有历史点云均对应两个列向量，通过两个KD树来保存所有历史点云的列向量，采用最邻近节点算法在KD树中进行搜索，通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的N帧历史点云，共得到2N帧候选点云。
在上述方法实施例内容的基础上，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤5通过计算两帧点云全局描述符的相似度来判断两帧点云是否发生回环，当前点云pc经过编码后得到全局描述符包含两个矩阵
Figure BDA0003541999030000033
历史点云中的某帧候选点云pq经过编码后同样得到两个矩阵
Figure BDA0003541999030000034
分别计算两帧点云第一层矩阵之间的相似度和第二层矩阵之间的相似度相加，即矩阵
Figure BDA0003541999030000035
与
Figure BDA0003541999030000036
的相似度加上矩阵
Figure BDA0003541999030000037
与
Figure BDA0003541999030000038
的相似度来衡量两帧点云之间的相似关系，矩阵的相似度通过余弦距离来计算，矩阵
Figure BDA0003541999030000039
与
Figure BDA00035419990300000310
之间的相似度S1：
Figure BDA00035419990300000311
其中，
Figure BDA00035419990300000312
和
Figure BDA00035419990300000313
分别表示矩阵
Figure BDA00035419990300000314
与
Figure BDA00035419990300000315
的列向量，第二层矩阵之间的相似度S2可以通过相同的方式计算得到，设置一个相似度的阈值τ，当S1+S2≥τ的时，则认为这两帧点云构成回环，进行回环修正，否则，回环检测失败，进行下一关键帧的回环检测。
第二方面，本发明的实施例提供了一种基于点云强度与高度信息的回环检测装置，包括：第一主模块，用于步骤1：采用SLAM得到的点云位姿，将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下；步骤2：通过点云高度信息编码和点云强度信息编码，对同一帧点云的几何与强度信息进行编码来构建点云的全局描述符；第二主模块，用于步骤3：将全局描述符中的两层矩阵转换为两个列向量，取矩阵每行所有元素的平均值作为列向量每行的值；第三主模块，用于步骤4：用两个KD树分别来保存所有历史点云的列向量；通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的若干帧历史点云；第四主模块，用于步骤5：计算当前点云和所有候选点云的相似度，选出其中最大的相似度，若相似度大于阈值，则回环检测成功，进行回环修正；否则，回环检测失败，进入下一帧。
第三方面，本发明的实施例提供了一种电子设备，包括：
至少一个处理器；以及
与处理器通信连接的至少一个存储器，其中：
存储器存储有可被处理器执行的程序指令，处理器调用程序指令能够执行第一方面的各种实现方式中任一种实现方式所提供的基于点云强度与高度信息的回环检测方法。
第四方面，本发明的实施例提供了一种非暂态计算机可读存储介质，非暂态计算机可读存储介质存储计算机指令，计算机指令使计算机执行第一方面的各种实现方式中任一种实现方式所提供的基于点云强度与高度信息的回环检测方法。
本发明实施例提供的基于点云强度与高度信息的回环检测方法及设备，通过采用SLAM过程估计位姿，对每帧点云进行一个初始的姿态变换，明显减少为克服旋转不变性问题产生的计算，和相关技术比较，在数据集实验中整体上表现出了更加准确的回环检测结果和更快的运行速度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做一简单的介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的基于点云强度与高度信息的回环检测方法流程图；
图2为本发明实施例提供的基于点云强度与高度信息的回环检测装置结构示意图；
图3为本发明实施例提供的电子设备的实体结构示意图；
图4a为本发明实施例提供的一帧原始的三维点云示意图；
图4b为本发明实施例提供的全局描述符效果示意图；
图5为本发明实施例提供的点云划分区域效果示意图；
图6为本发明实施例提供的KITTI数据集中05序列效果示意图；
图7为本发明实施例提供的KITTI数据集中08序列效果示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。另外，本发明提供的各个实施例或单个实施例中的技术特征可以相互任意结合，以形成可行的技术方案，这种结合不受步骤先后次序和/或结构组成模式的约束，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时，应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
本发明实施例提供了一种基于点云强度与高度信息的回环检测方法，参见图1，该方法包括：步骤1：采用SLAM得到的点云位姿，将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下；步骤2：通过点云高度信息编码和点云强度信息编码，对同一帧点云的几何与强度信息进行编码来构建点云的全局描述符；步骤3：将全局描述符中的两层矩阵转换为两个列向量，取矩阵每行所有元素的平均值作为列向量每行的值；步骤4：用两个KD树分别来保存所有历史点云的列向量；通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的若干帧历史点云；步骤5：计算当前点云和所有候选点云的相似度，选出其中最大的相似度，若相似度大于阈值，则回环检测成功，进行回环修正；否则，回环检测失败，进入下一帧。
基于上述方法实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下，包括：通过旋转矩阵将当前帧点云姿态从雷达坐标系下变换为全局坐标系下的姿态。
基于上述方法实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤2中的全局描述符的第一层是通过将点云投影到二维平面进行编码得到对应的矩阵得到的，包括：对于一帧激光点云数据，定义一距离阈值Dmax，取点云中距离点云中心小于阈值Dmax的点，得到点云P；通过半径将点云P在xy平面上均匀的划分为若干扇形区域，扇形数量为Nr；用相等距离同心圆将所有扇形划分为若干区域，设同心圆相距为Dc，则每个扇形被划分为Nc＝Dmax/Dc个区域；将点云划分为Nc×Nr个区域，用Emn表示所有区域，则m和n分别满足：
Figure BDA0003541999030000051
N为自然数集，m为第m个扇形区域，n为第n圈同心圆，划分得到的所有区域和大小为Nc×Nr的矩阵的元素一一对应；取每个区域中最高点的高度值为矩阵中对应元素的值，得到高度矩阵，高度矩阵每个元素的取值可以表示为：
Figure BDA0003541999030000061
其中Z(pn)函数为点pn在Z轴上的取值，p(Emn)表示的是区域Emn内的所有点，如果Emn区域内没有点，则对应矩阵元素取值为0。
在另一实施例中，两种不同的编码方式，是通过点云高度信息编码和点云强度信息编码，对同一帧点云的几何与强度信息进行编码，生成的对应全局描述符。如图4a所示为一帧原始的三维点云，本发明算法生成的对应全局描述符，如图4b所示，由上下两层矩阵构成。
所述全局描述符的第一层，是通过将点云投影到二维平面进行编码得到对应的矩阵，即采用点云高度信息编码，算法原理步骤如下：
1)对于一帧激光点云数据，定义一个距离阈值Dmax，取点云中距离点云中心小于阈值Dmax的点，得到点云P(p1，p2，…，pn)；
2)通过半径将点云p在xy平面上，均匀的划分为若干扇形区域，扇形数量为Nr；
3)用相等距离同心圆将所有扇形划分为若干区域，设同心圆相距为Dc，则每个扇形被划分为Nc＝Dmax/Dc个区域；
4)点云被划分为了个Nc×Nr区域，用Emn来表示所有区域(如图5所示)，则m和n分别满足(1)式，划分得到的所有区域和大小Nc×Nr为的矩阵的元素一一对应；
5)取每个区域中最高点的高度值为矩阵中对应元素的值，得到高度矩阵，高度矩阵每个元素的取值可以表示为(2)式所示。
基于上述方法实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤2中的全局描述符的第二层采用点云的强度信息构建，包括：将点云P中的点转换到球面坐标系；再将所有的点对应到大小为h×w的矩阵MI中，点云中每个点pn＝(x,y,z)和矩阵MI元素的映射关系可以表示为：
Figure BDA0003541999030000062
其中，r为点到点云中心的距离，f表示的是激光雷达垂直方向上的视角，w和h代表对应矩阵的列数与行数，u和v分别代表点所对应元素在矩阵中的行与列；将点云P中点的强度值赋予给矩阵MI所对应元素。
在另一实施例中，得到的这样一个矩阵MH来表示一帧点云，其构成回环检测算法全局描述符的第一层。通过几何信息对点云编码后得到矩阵如图4b中第一层矩阵所示。
所述步骤2全局描述符的第二层利用点云的强度信息构建，即点云强度信息编码，算法原理步骤如下：
(1)将点云P中的点转换到球面坐标系；
(2)再将所有的点对应到大小为h×w的矩阵MI中，点云中每个点pn＝(x,y,z)和矩阵MI元素的映射关系可以表示为(3)式所示。将点云中所有点对应到了矩阵之后，将点的强度值赋予矩阵对应元素。例如将KITTI数据集中的一帧点云，按上述方式映射得到的矩阵如图4b中第二层所示。因为KITTI数据集使用的是64线三维激光雷达，所以对应矩阵的行数取h＝64、w＝900。
基于上述方法实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤3中的将全局描述符中的两层矩阵转换为两个列向量，包括：将全局描述符中的两层矩阵MH、MI转换为两个列向量VH、VI，取矩阵每行所有元素的平均值作为列向量每行的值，包括：
Figure BDA0003541999030000071
Figure BDA0003541999030000072
其中，i为矩阵的第i行，即列向量的第i行,n为矩阵的第n列。
基于上述方法实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤4中的所有历史点云均对应两个列向量，通过两个KD树来保存所有历史点云的列向量，采用最邻近节点算法在KD树中进行搜索，通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的N帧历史点云，共得到2N帧候选点云。
基于上述方法实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测方法，所述步骤5通过计算两帧点云全局描述符的相似度来判断两帧点云是否发生回环，当前点云pc经过编码后得到全局描述符包含两个矩阵
Figure BDA0003541999030000081
历史点云中的某帧候选点云pq经过编码后同样得到两个矩阵
Figure BDA0003541999030000082
分别计算两帧点云第一层矩阵之间的相似度和第二层矩阵之间的相似度相加，即矩阵
Figure BDA0003541999030000083
与
Figure BDA0003541999030000084
的相似度加上矩阵
Figure BDA0003541999030000085
与
Figure BDA0003541999030000086
的相似度来衡量两帧点云之间的相似关系，矩阵的相似度通过余弦距离来计算，矩阵
Figure BDA0003541999030000087
与
Figure BDA0003541999030000088
之间的相似度S1：
Figure BDA0003541999030000089
其中，
Figure BDA00035419990300000810
和
Figure BDA00035419990300000811
分别表示矩阵
Figure BDA00035419990300000812
与
Figure BDA00035419990300000813
的列向量，第二层矩阵之间的相似度S2可以通过相同的方式计算得到，设置一个相似度的阈值τ，当S1+S2≥τ的时，则认为这两帧点云构成回环，进行回环修正，否则，回环检测失败，进行下一关键帧的回环检测。
本发明实施例提供的基于点云强度与高度信息的回环检测方法，通过采用SLAM过程估计位姿，对每帧点云进行一个初始的姿态变换，明显减少为克服旋转不变性问题产生的计算，和相关技术比较，在数据集实验中整体上表现出了更加准确的回环检测结果和更快的运行速度。
为验证本发明技术方法的可行性，选取KITTI数据集中的回环数比较多的05序列和08序列进行实施例所述方案的实现，具体结果如图6(05序列)和图7(08序列)所示，图中为使用本方法(折点图)与scan context算法(折线图)进行回环检测，在不同阈值τ下的准确率-召回率曲线的对比。准确率(precision竖轴)和召回率(recall横轴)的计算公式为：
Figure BDA00035419990300000814
Figure BDA00035419990300000815
其中，TP为算法检测是回环的次数，FN为算法检测不是回环的次数，FP为事实是回环的出现次数，TN的事实不是回环的出现次数。该对比图充分说明本方法的合理性以及有效性。
本发明各个实施例的实现基础是通过具有处理器功能的设备进行程序化的处理实现的。因此在工程实际中，可以将本发明各个实施例的技术方案及其功能封装成各种模块。基于这种现实情况，在上述各实施例的基础上，本发明的实施例提供了一种基于点云强度与高度信息的回环检测装置，该装置用于执行上述方法实施例中的基于点云强度与高度信息的回环检测方法。参见图2，该装置包括：第一主模块，用于步骤1：采用SLAM得到的点云位姿，将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下；步骤2：通过点云高度信息编码和点云强度信息编码，对同一帧点云的几何与强度信息进行编码来构建点云的全局描述符；第二主模块，用于步骤3：将全局描述符中的两层矩阵转换为两个列向量，取矩阵每行所有元素的平均值作为列向量每行的值；第三主模块，用于步骤4：用两个KD树分别来保存所有历史点云的列向量；通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的若干帧历史点云；第四主模块，用于步骤5：计算当前点云和所有候选点云的相似度，选出其中最大的相似度，若相似度大于阈值，则回环检测成功，进行回环修正；否则，回环检测失败，进入下一帧。
本发明实施例提供的基于点云强度与高度信息的回环检测装置，采用图2中的若干模块，通过采用SLAM过程估计位姿，对每帧点云进行一个初始的姿态变换，明显减少为克服旋转不变性问题产生的计算，和相关技术比较，在数据集实验中整体上表现出了更加准确的回环检测结果和更快的运行速度。
需要说明的是，本发明提供的装置实施例中的装置，除了可以用于实现上述方法实施例中的方法外，还可以用于实现本发明提供的其他方法实施例中的方法，区别仅仅在于设置相应的功能模块，其原理与本发明提供的上述装置实施例的原理基本相同，只要本领域技术人员在上述装置实施例的基础上，参考其他方法实施例中的具体技术方案，通过组合技术特征获得相应的技术手段，以及由这些技术手段构成的技术方案，在保证技术方案具备实用性的前提下，就可以对上述装置实施例中的装置进行改进，从而得到相应的装置类实施例，用于实现其他方法类实施例中的方法。例如：
基于上述装置实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测装置，还包括：第一子模块，用于实现所述将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下，包括：通过旋转矩阵将当前帧点云姿态从雷达坐标系下变换为全局坐标系下的姿态。
基于上述装置实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测装置，还包括：第二子模块，用于实现所述步骤2中的全局描述符的第一层是通过将点云投影到二维平面进行编码得到对应的矩阵得到的，包括：对于一帧激光点云数据，定义一距离阈值Dmax，取点云中距离点云中心小于阈值Dmax的点，得到点云P；通过半径将点云P在xy平面上均匀的划分为若干扇形区域，扇形数量为Nr；用相等距离同心圆将所有扇形划分为若干区域，设同心圆相距为Dc，则每个扇形被划分为Nc＝Dmax/Dc个区域；将点云划分为Nc×Nr个区域，用Emn表示所有区域，则m和n分别满足：
Figure BDA0003541999030000101
N为自然数集，划分得到的所有区域和大小为Nc×Nr的矩阵的元素一一对应；取每个区域中最高点的高度值为矩阵中对应元素的值，得到高度矩阵，高度矩阵每个元素的取值可以表示为：
Figure BDA0003541999030000102
其中Z(pn)函数为点pn在Z轴上的取值，p(Emn)表示的是区域Emn内的所有点，如果Emn区域内没有点，则对应矩阵元素取值为0。
基于上述装置实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测装置，还包括：第三子模块，用于实现所述步骤2中的全局描述符的第二层采用点云的强度信息构建，包括：将点云P中的点转换到球面坐标系；再将所有的点对应到大小为h×w的矩阵MI中，点云中每个点pn＝(x,y,z)和矩阵MI元素的映射关系可以表示为：
Figure BDA0003541999030000103
其中，r为点到点云中心的距离，f表示的是激光雷达垂直方向上的视角，w和h代表对应矩阵的列数与行数，u和v分别代表点所对应元素在矩阵中的行与列；将点云P中点的强度值赋予给矩阵MI所对应元素。
基于上述装置实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测装置，还包括：第四子模块，用于实现所述步骤3中的将全局描述符中的两层矩阵转换为两个列向量，包括：将全局描述符中的两层矩阵MH、MI转换为两个列向量VH、VI，取矩阵每行所有元素的平均值作为列向量每行的值，包括：
Figure BDA0003541999030000104
Figure BDA0003541999030000111
其中，i为矩阵的第i行，即列向量的第i行,n为矩阵的第n列。
基于上述装置实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测装置，还包括：第五子模块，用于实现所述步骤4中的所有历史点云均对应两个列向量，通过两个KD树来保存所有历史点云的列向量，采用最邻近节点算法在KD树中进行搜索，通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的N帧历史点云，共得到2N帧候选点云。
基于上述装置实施例的内容，作为一种可选的实施例，本发明实施例中提供的基于点云强度与高度信息的回环检测装置，还包括：第六子模块，用于实现所述步骤5通过计算两帧点云全局描述符的相似度来判断两帧点云是否发生回环，当前点云pc经过编码后得到全局描述符包含两个矩阵
Figure BDA0003541999030000112
历史点云中的某帧候选点云pq经过编码后同样得到两个矩阵
Figure BDA0003541999030000113
Figure BDA0003541999030000114
分别计算两帧点云第一层矩阵之间的相似度和第二层矩阵之间的相似度相加，即矩阵
Figure BDA0003541999030000115
与
Figure BDA0003541999030000116
的相似度加上矩阵
Figure BDA0003541999030000117
与
Figure BDA0003541999030000118
的相似度来衡量两帧点云之间的相似关系，矩阵的相似度通过余弦距离来计算，矩阵
Figure BDA0003541999030000119
与
Figure BDA00035419990300001110
之间的相似度S1：
Figure BDA00035419990300001111
其中，
Figure BDA00035419990300001112
和
Figure BDA00035419990300001113
分别表示矩阵
Figure BDA00035419990300001114
与
Figure BDA00035419990300001115
的列向量，第二层矩阵之间的相似度S2可以通过相同的方式计算得到，设置一个相似度的阈值τ，当S1+S2≥τ的时，则认为这两帧点云构成回环，进行回环修正，否则，回环检测失败，进行下一关键帧的回环检测。
本发明实施例的方法是依托电子设备实现的，因此对相关的电子设备有必要做一下介绍。基于此目的，本发明的实施例提供了一种电子设备，如图3所示，该电子设备包括：至少一个处理器(processor)、通信接口(Communications Interface)、至少一个存储器(memory)和通信总线，其中，至少一个处理器，通信接口，至少一个存储器通过通信总线完成相互间的通信。至少一个处理器可以调用至少一个存储器中的逻辑指令，以执行前述各个方法实施例提供的方法的全部或部分步骤。
此外，上述的至少一个存储器中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个方法实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件实现。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。
附图中的流程图和框图显示了根据本发明的多个实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。基于这种认识，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
在本专利中，术语"包括"、"包含"或者其任何其它变体意在涵盖非排它性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其它要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句"包括……"限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种基于点云强度与高度信息的回环检测方法，其特征在于，包括：步骤1：采用SLAM得到的点云位姿，将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下；步骤2：通过点云高度信息编码和点云强度信息编码，对同一帧点云的几何与强度信息进行编码来构建点云的全局描述符；步骤3：将全局描述符中的两层矩阵转换为两个列向量，取矩阵每行所有元素的平均值作为列向量每行的值；步骤4：用两个KD树分别来保存所有历史点云的列向量；通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的若干帧历史点云；步骤5：计算当前点云和所有候选点云的相似度，选出其中最大的相似度，若相似度大于阈值，则回环检测成功，进行回环修正；否则，回环检测失败，进入下一帧。
2.根据权利要求1所述的基于点云强度与高度信息的回环检测方法，其特征在于，所述将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下，包括：通过旋转矩阵将当前帧点云姿态从雷达坐标系下变换为全局坐标系下的姿态。
3.根据权利要求2所述的基于点云强度与高度信息的回环检测方法，其特征在于，所述步骤2中的全局描述符的第一层是通过将点云投影到二维平面进行编码得到对应的矩阵得到的，包括：对于一帧激光点云数据，定义一距离阈值Dmax，取点云中距离点云中心小于阈值Dmax的点，得到点云P；通过半径将点云P在xy平面上均匀的划分为若干扇形区域，扇形数量为Nr；用相等距离同心圆将所有扇形划分为若干区域，设同心圆相距为Dc，则每个扇形被划分为Nc＝Dmax/Dc个区域；将点云划分为Nc×Nr个区域，用Emn表示所有区域，则m和n分别满足：
Figure FDA0003541999020000011
N为自然数集，划分得到的所有区域和大小为Nc×Nr的矩阵的元素一一对应；取每个区域中最高点的高度值为矩阵中对应元素的值，得到高度矩阵，高度矩阵每个元素的取值可以表示为：
Figure FDA0003541999020000012
其中Z(pn)函数为点pn在Z轴上的取值，p(Emn)表示的是区域Emn内的所有点，如果Emn区域内没有点，则对应矩阵元素取值为0。
4.根据权利要求3所述的基于点云强度与高度信息的回环检测方法，其特征在于，所述步骤2中的全局描述符的第二层采用点云的强度信息构建，包括：将点云P中的点转换到球面坐标系；再将所有的点对应到大小为h×w的矩阵MI中，点云中每个点pn＝(x,y,z)和矩阵MI元素的映射关系可以表示为：
Figure FDA0003541999020000021
其中，r为点到点云中心的距离，f表示的是激光雷达垂直方向上的视角，w和h代表对应矩阵的列数与行数，u和v分别代表点所对应元素在矩阵中的行与列；将点云P中点的强度值赋予给矩阵MI所对应元素。
5.根据权利要求4所述的基于点云强度与高度信息的回环检测方法，其特征在于，所述步骤3中的将全局描述符中的两层矩阵转换为两个列向量，包括：将全局描述符中的两层矩阵MH、MI转换为两个列向量VH、VI，取矩阵每行所有元素的平均值作为列向量每行的值，包括：
Figure FDA0003541999020000022
Figure FDA0003541999020000023
其中，i为矩阵的第i行，即列向量的第i行,n为矩阵的第n列。
6.根据权利要求5所述的基于点云强度与高度信息的回环检测方法，其特征在于，所述步骤4中的所有历史点云均对应两个列向量，通过两个KD树来保存所有历史点云的列向量，采用最邻近节点算法在KD树中进行搜索，通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的N帧历史点云，共得到2N帧候选点云。
7.根据权利要求6所述的基于点云强度与高度信息的回环检测方法，其特征在于，所述步骤5通过计算两帧点云全局描述符的相似度来判断两帧点云是否发生回环，当前点云pc经过编码后得到全局描述符包含两个矩阵
Figure FDA0003541999020000031
历史点云中的某帧候选点云pq经过编码后同样得到两个矩阵M
Figure FDA0003541999020000032
分别计算两帧点云第一层矩阵之间的相似度和第二层矩阵之间的相似度相加，即矩阵
Figure FDA0003541999020000033
与
Figure FDA0003541999020000034
的相似度加上矩阵
Figure FDA0003541999020000035
与
Figure FDA0003541999020000036
的相似度来衡量两帧点云之间的相似关系，矩阵的相似度通过余弦距离来计算，矩阵
Figure FDA0003541999020000037
与
Figure FDA0003541999020000038
之间的相似度S1：
Figure FDA0003541999020000039
其中，
Figure FDA00035419990200000310
和
Figure FDA00035419990200000311
分别表示矩阵
Figure FDA00035419990200000312
与
Figure FDA00035419990200000313
的列向量，第二层矩阵之间的相似度S2可以通过相同的方式计算得到，设置一个相似度的阈值τ，当S1+S2≥τ的时，则认为这两帧点云构成回环，进行回环修正，否则，回环检测失败，进行下一关键帧的回环检测。
8.一种基于点云强度与高度信息的回环检测装置，其特征在于，包括：第一主模块，用于步骤1：采用SLAM得到的点云位姿，将激光雷达获得的点云从雷达坐标系下变换到世界坐标系下；步骤2：通过点云高度信息编码和点云强度信息编码，对同一帧点云的几何与强度信息进行编码来构建点云的全局描述符；第二主模块，用于步骤3：将全局描述符中的两层矩阵转换为两个列向量，取矩阵每行所有元素的平均值作为列向量每行的值；第三主模块，用于步骤4：用两个KD树分别来保存所有历史点云的列向量；通过当前点云的两个列向量，分别在两个KD树中各找到和当前点云预发生回环的若干帧历史点云；第四主模块，用于步骤5：计算当前点云和所有候选点云的相似度，选出其中最大的相似度，若相似度大于阈值，则回环检测成功，进行回环修正；否则，回环检测失败，进入下一帧。
9.一种电子设备，其特征在于，包括：
至少一个处理器、至少一个存储器和通信接口；其中，
所述处理器、存储器和通信接口相互间进行通信；
所述存储器存储有可被所述处理器执行的程序指令，所述处理器调用所述程序指令，以执行权利要求1至7任一项权利要求所述的方法。
10.一种非暂态计算机可读存储介质，其特征在于，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行权利要求1至7中任一项权利要求所述的方法。
CN202210235580.7A 2022-03-11 2022-03-11 基于点云强度与高度信息的回环检测方法及设备 Pending CN115047487A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210235580.7A CN115047487A (zh) 2022-03-11 2022-03-11 基于点云强度与高度信息的回环检测方法及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210235580.7A CN115047487A (zh) 2022-03-11 2022-03-11 基于点云强度与高度信息的回环检测方法及设备

Publications (1)

Publication Number Publication Date
CN115047487A true CN115047487A (zh) 2022-09-13

Family

ID=83158098

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210235580.7A Pending CN115047487A (zh) 2022-03-11 2022-03-11 基于点云强度与高度信息的回环检测方法及设备

Country Status (1)

Country Link
CN (1) CN115047487A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115601434A (zh) * 2022-12-12 2023-01-13 安徽蔚来智驾科技有限公司（Cn） 回环检测方法、计算机设备、计算机可读存储介质及车辆
CN116358532A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN116452422A (zh) * 2023-04-24 2023-07-18 上海几何伙伴智能驾驶有限公司 一种4d成像毫米波雷达的回环检测方法

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115601434A (zh) * 2022-12-12 2023-01-13 安徽蔚来智驾科技有限公司（Cn） 回环检测方法、计算机设备、计算机可读存储介质及车辆
CN115601434B (zh) * 2022-12-12 2023-03-07 安徽蔚来智驾科技有限公司 回环检测方法、计算机设备、计算机可读存储介质及车辆
CN116452422A (zh) * 2023-04-24 2023-07-18 上海几何伙伴智能驾驶有限公司 一种4d成像毫米波雷达的回环检测方法
CN116452422B (zh) * 2023-04-24 2024-02-20 上海几何伙伴智能驾驶有限公司 一种4d成像毫米波雷达的回环检测方法
CN116358532A (zh) * 2023-05-31 2023-06-30 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆
CN116358532B (zh) * 2023-05-31 2023-09-26 小米汽车科技有限公司 回环检测方法、装置、存储介质以及车辆

Similar Documents

Publication Publication Date Title
CN115047487A (zh) 基于点云强度与高度信息的回环检测方法及设备
US11181624B2 (en) Method and apparatus for calibration between laser radar and camera, device and storage medium
US10685201B2 (en) Method and system for recognizing location information in two-dimensional code
JP4372477B2 (ja) ２分木を巡回する方法およびシステム
CN109712071B (zh) 基于航迹约束的无人机图像拼接与定位方法
CN113191372A (zh) 一种舰船目标有向检测模型的构建方法及应用
CN112562000A (zh) 基于特征点检测和误匹配筛选的机器人视觉定位方法
CN112560962A (zh) 骨骼动画的姿态匹配方法、装置、电子设备及存储介质
CN112734837B (zh) 图像匹配的方法及装置、电子设备及车辆
CN114663579A (zh) 孪生三维模型生成方法、装置、电子设备及存储介质
CN112632399B (zh) 一种基于空间位置的拓扑关系获取方法、装置和存储介质
CN112132951B (zh) 一种基于视觉的网格语义地图的构建方法
CN114565953A (zh) 图像处理方法、装置、电子设备及计算机可读存储介质
CN115730156B (zh) 一种船舶新闻到轨迹的匹配系统及方法
CN112541902A (zh) 相似区域搜索方法、装置、电子设备及介质
CN111539435A (zh) 语义分割模型构建方法及图像分割方法、设备、存储介质
CN116091784A (zh) 一种目标跟踪方法、设备及存储介质
CN113793364A (zh) 目标跟踪方法、装置、计算机设备和存储介质
CN114494398A (zh) 倾斜目标的处理方法、装置、存储介质与处理器
CN115147511A (zh) 一种边缘构件详图生成方法、装置、设备及可读介质
CN109189773B (zh) 一种数据修复方法及装置
CN116628251B (zh) 月球表面安全区域的搜索方法、装置、设备及介质
CN112288759A (zh) 一种边界提取方法、装置、设备及存储介质
CN114332345B (zh) 一种基于双目视觉的冶金库区局部三维重建方法及系统
Liu et al. Line simplification algorithm implementation and error analysis

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