一种利用语义拓扑描述符进行三维点云的闭环检测方法

1.本发明涉及视觉空间定位领域，尤其涉及一种基于语义拓扑描述符的三维点云闭环检测方法。


背景技术：

2.随着室内外机器人的应用越来越广泛，自动驾驶和增强现实应用的不断推广，如何实现机器人在先验地图中的重定位成为急需解决的问题。机器人同时定位与建图技术(simultaneous localization and mapping,slam)通过对输入的传感器数据进行处理，获得载体位姿并构建出环境地图。
3.闭环检测作为同时定位和建图的重要组成部分，可以帮助机器人识别以前访问过的位置，纠正累积的漂移误差，并构建全局一致的地图，为机器人提供准确的先验信息。
4.文献《zhuy,may,chen l,et al.gosmatch:graph-of-semantics matching for detecting loop closures in 3d lidar data[c]//2020 ieee/rsj international conference on intelligent robots and systems(iros).ieee,2020:5151-5157.》从原始激光三维点云中提取出了车辆、树和杆状物作为语义物体，将每种物体的聚类中心作为节点，连接节点构建无向图并通过距离直方图完成闭环帧间的相似度匹配，该方法选择的语义种类非常有限，因此不能充分利用语义信息，使其对分辨场景独特性的能力受到了限制。
[0005]
文献《kong x,yang x,zhai g,et al.semantic graph based place recognition for 3d point clouds[c]//2020 ieee/rsj international conference on intelligent robots and systems(iros).ieee,2020:8216-8223.》首先利用语义分割网络提取出每个三维点云的类别标签，从中选择了12种类型的语义信息，然后将三维点云的聚类中心输入到设计好的神经网络中，完成图的相似性匹配，该方法对所有大规模的语义三维点云，如地面、建筑物、草坪等三维点云也只提取出一个聚类中心，因此在不同视角下提取的特征会有所差异，且无法解决三维点云数据中存在的物体分段问题。
[0006]
文献《vidanapathirana k,moghadam p,harwood b,et al.locus:lidar-based place recognition using spatiotemporal higher-order pooling[c]//2021ieee international conference on robotics and automation(icra).ieee,2021:5075-5081.》对场景中的空间拓扑关系和时间信息进行特征提取，并通过对多级特征进行非线性变换的二阶池化，最终得到一个固定长度的特征描述符，该方法没有考虑室外环境中，动态对象的位置随机性会造成三维点云遮挡、丢失等影响，从而影响环境的表达和特征描述符的生成。
[0007]
在现有的闭环检测技术中，只根据原始三维点云的三维分布提取几何特征，或关注于单个语义物体三维点云的特征而忽略了语义物体之间的关联信息，这些方案容易受到动态物体和视角变化的影响。为解决现有技术中存在的问题。


技术实现要素：

[0008]
本发明提出了一种利用语义拓扑描述符进行三维点云的闭环检测算法，通过语义分割提取环境中的语义信息，并融合单个语义物体的特征和多个语义物体的联系，提高了闭环检测的精度和鲁棒性。
[0009]
为实现上述目的，本发明采取的技术方案为：
[0010]
一种利用语义拓扑描述符进行三维点云的闭环检测方法，包括以下步骤：
[0011]
s1:通过语义分割提取三维点云的标签类别，并根据三维点云的语义标签丢弃指定的物体三维点云；
[0012]
s2:通过凸壳投影将三维点云进行空间变换，根据空间变换后三维点云空间分布判断点的凹凸性，将凸性点对应的原始三维点云空间中的点作为特征点；
[0013]
s3:根据原始空间中三维点云的几何信息和语义信息计算特征点的分数，利用鸟瞰图划分栅格，根据栅格位置选取特征点作为语义拓扑图的节点，将每个节点之间的最大空间距离作为语义拓扑图的边，构建语义拓扑图；
[0014]
s4:将语义拓扑图转换为矩阵形式的描述符，通过两阶段搜索策略，以栅格图径向角度作为起点，拉伸为二维矩阵，以语义距离作为矩阵中对应行列的数值，构建为矩阵形式的描述符，完成闭环检测。
[0015]
所述s1中，通过激光雷达获取原始的三维点云数据，将原始的三维点云数据通过三维点云数据的语义分割网络得到每个三维点云的语义类别；
[0016]
所述语义分割采用rangenet++网络；在语义分割后的三维点云中，丢弃具有动态语义对象的三维点云数据，保留场景中静态的三维点云数据。
[0017]
所述语义分割后的三维点云中，还包括丢弃动态语义对象对应的静态三维点云数据。
[0018]
所述凸壳投影，将三维点云进行体素降采样，三维点云通过式(1)进行空间变换；设定原始三维点云集合p
origin
、投影后的三维点云集合p
hull
及凸点集合p
convex
，对应每个集合中的三维点云表示为po、ph及pc，观测点中心表示为pv，投影公式如式(1)所示：
[0019][0020]
其中：
[0021][0022]
其中，f为核函数，γ表示系数因子；
[0023]
所述判断点的凹凸性，令k为设定提取的最近邻点数量，在空间变换后的三维点云中筛选出最近邻的k个点提取出满足公式(3)的凸性特征点，：
[0024][0025]
其中，每个三维点云包括多个特征点，k表示最近邻点的排序序号，其中k∈{1,2,
…
,k}，表示对应k序号的投影点。
[0026]
所述计算特征点的分数，将三维点云的距离分布作为几何信息设定特征点与观测中心的距离为dh，场景中三维点云的距离均值为方差为σ2，通过高斯核函数计算
特征点的几何分数φ1(ph)，如式(4)所示：
[0027][0028]
特征点周围近邻点有相同语义类别的三维点云越多，则该特征点的语义分数φ2(ph)越高，反之则语义分数越低，如式(5)所示：
[0029][0030]
其中，α表示系数因子，将特征点的几何分数φ1(ph)和语义分数φ2(ph)相乘得到特征点总分数φ(ph)，如式(6)所示：
[0031]
φ(ph)＝φ1(ph)*φ2(ph)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0032]
所述构建语义拓扑图，采用鸟瞰图，以观测中心为圆心，半径由近到远等距的划分nr个同心圆，并且以观测前方作为起始角度，顺时针方向等间隔的径向划分ns个区域，同心圆划分线和径向区域划分线组合，将三维点云空间以从上向下的俯视角度将场景三维点云划分为多个栅格，将同一栅格中不同高度的特征点，通过局部极大值抑制选取最大分数的特征点作为所在栅格的代表点，所有栅格中的代表点作为语义拓扑图的节点；以栅格作为距离的基本单位，将任意两个栅格距离与特征点总分数相乘得到语义拓扑图的节点之间带有几何和语义权重的语义距离，取节点之间最大的语义距离作为图的边，构建出语义拓扑图；
[0033]
设定任意栅格的特征点为n
i,m
，其中i是栅格圈数的序号，i∈{1,2,
…
,nr}，nr为栅格圈数的最大序号；j表示栅格角度划分的第j序号，j∈{1,2,
…
,ns}，ns为栅格角度划分的最大序号；m表示栅格角度划分的第m序号，且m≠j；栅格特征点的语义类别为o，设定max为使得语义距离最大的函数，语义几何距离dg和语义种类距离ds计算如式(7)、式(8)所示：
[0034][0035][0036]
其中，o表示栅格特征点的语义类别。
[0037]
所述两步搜索算法，包括第一步搜索算法及第二步搜索算法；以观测中心为圆心的栅格地图由多个同心圆组成，径向间隔相同的角度进行划分，拉伸为首尾相接的二维矩阵；
[0038]
所述第一步搜索算法，将矩阵转换为向量k，采用k-d树(k-demension tree)的结构存储和搜索当前场景的候选帧；
[0039]
所述采用k-d树(k-demension tree)的结构存储和搜索当前场景的候选帧，包括：向量k的第一个值从矩阵的第一行获得，后面的值依次从下一行获得，设定no为所选语义种类的数量，nr为栅格划分圈数，为语义种类相同的栅格特征点的ds平均值，为圈数序号相同的栅格特征点的dg平均值，所有行在编码后生成nr+no维向量ε，如式(9)所示：
[0040][0041]
其中，r表示矩阵的行数，o是语义类别，
[0042]
所述第二步搜索算法，设定当前场景的矩阵描述符为sqg，栅格圈数为ni的特征向量为sqni，||sqni||代表其二范数，语义种类为sql，候选场景的矩阵描述符为scg，栅格圈数
为ni的特征向量为scni，||sqni||代表其二范数，语义种类为scl，矩阵描述符中每个单位中存储的值为语义距离，使用余弦距离计算描述符之间的差异性，并且添加了语义信息的约束，d(sqg,scg)为矩阵描述符之间的相似度，计算公式如式(10)所示：
[0043][0044]
其中，为矩阵描述符每一行的相似度计算，如式(11)所示：
[0045][0046]
将计算得到的相似度和设定相似度阈值相比较，若相似度小于设定的相似度阈值，则当前场景存在对应的闭环场景，反之则当前场景不存在对应的闭环场景，闭环检测部分完成，将结果作用于同时定位和建图的过程中。
[0047]
有益技术效果
[0048]
1、本发明提出一种利用语义拓扑描述符进行三维点云的闭环检测算法，针对以往基于单一信息进行闭环检测的算法鲁棒性较差的问题，在对三维点云进行语义分割后，数据预处理部分丢弃了动态语义对象的三维点云，以减少动态效果和计算负担。
[0049]
2、本发明从保留的三维点云中获得静态的特征点，并根据语义特征和距离分布计算相应的分数，以此表达特征点的重要程度。
[0050]
3、本发明通过非最大值抑制(nms)利用鸟眼投影来提取节点并构建语义拓扑图；将语义拓扑图转换为矩阵描述符，并使用两步搜索策略进行闭环检测，在保证精度的同时提高匹配的速度。
[0051]
4、本发明能够有效应对视角变换或动态场景下的闭环检测工作，以服务于无人驾驶和地图构建等应用场景。
[0052]
5、为了验证算法能够取得很好的匹配精度，本发明在室外数据集kitti上进行测试，在kitti00、02、05、06、07、08序列中存在大量的闭环场景，本发明分别取得了0.984、0.843、0.970、0.991、0.915、0.897的f1最大分数，此外在具有动态物体的闭环场景中，本发明的f1分数也保持在0.8以上。为了验证算法计算得到的闭环场景漂移误差更小，在kitti00、02、05、06、07、08数据集上进行测试，其在偏航角上的误差分别为0.226、1.083、0.492、0.070、0.199、1.204、0.546，单位为弧度。此外，本发明对计算耗时进行了实验测试，对于每一帧三维点云数据，平均的语义分割模块耗时为83ms，描述符计算耗时为18ms，闭环场景匹配耗时为45ms，能够满足实际系统的耗时要求。
附图说明
[0053]
图1为本发明实施例提供的一种利用语义拓扑描述符进行三维点云的闭环检测方法流程图；
[0054]
图2为本发明实施例提供的一种利用语义拓扑描述符进行三维点云的闭环检测方法处理过程示意图；
[0055]
图3为本发明实施例提供的特征点及节点提取算法流程图
[0056]
图4为本发明实例提供的球面投影示例图；
[0057]
图5为本发明实施例提供的闭环检测流程图；
[0058]
图6为本发明鸟眼投影划分的栅格图和转换的二维描述符示意图。
具体实施方式
[0059]
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。
[0060]
本实例数据来源是kitti数据集00序列其中的交通路口的三维点云数据。
[0061]
本实施例提出了一种利用语义拓扑描述符进行三维点云的闭环检测方法，如图1、图2所示，包括以下步骤：
[0062]
s1:通过语义分割提取三维点云的标签类别，并根据三维点云的语义标签丢弃指定的物体三维点云；
[0063]
所述s1中，通过激光雷达获取原始的三维点云数据，将原始的三维点云数据通过三维点云数据的语义分割网络得到每个三维点云的语义类别；
[0064]
所述语义分割采用rangenet++网络；在语义分割后的三维点云中，丢弃具有动态语义对象的三维点云数据，保留场景中静态的三维点云数据。
[0065]
s2:通过凸壳投影将三维点云进行空间变换，如图3、图4所示，根据空间变换后三维点云空间分布判断点的凹凸性，将凸性点对应的原始三维点云空间中的点作为特征点；
[0066]
所述语义分割后的三维点云中，还包括丢弃动态语义对象对应的静态三维点云数据。
[0067]
所述凸壳投影，将三维点云进行体素降采样，降低数据的计算量；之后三维点云通过式(1)进行空间变换；设定原始三维点云集合p
origin
、投影后的三维点云集合p
hull
及凸点集合p
convex
，对应每个集合中的三维点云表示为po、ph及pc，观测点中心表示为pv，投影公式如式(1)所示：
[0068][0069]
其中：
[0070][0071]
其中，f为核函数，γ表示系数因子，本实施例中设置为10000；
[0072]
所述判断点的凹凸性，令k为设定提取的最近邻点数量，在空间变换后的三维点云中筛选出最近邻的k个点提取出满足公式(3)的凸性特征点：
[0073][0074]
其中，每个三维点云包括多个特征点，k表示最近邻点的排序序号，其中k∈{1,2,
…
,k}，表示对应k序号的投影点；
[0075]
s3:根据原始空间中三维点云的几何信息和语义信息计算特征点的分数，利用鸟瞰图划分栅格，根据栅格位置选取特征点作为语义拓扑图的节点，并考虑特征点之间的空间距离，将每个节点之间的最大空间距离作为语义拓扑图的边，构建语义拓扑图；
[0076]
s4:将语义拓扑图转换为矩阵形式的描述符，如图5、图6所示，通过两阶段搜索策略，完成闭环检测。
[0077]
所述计算特征点的分数，为了评估特征点的稳定性，将三维点云的距离分布作为
几何信息，与均值距离越靠近的三维点云，在场景中稳定性越高，设定特征点与观测中心的距离为dh，场景中三维点云的距离均值为方差为σ2，通过高斯核函数计算特征点的几何分数φ1(ph)，如式(4)所示：
[0078][0079]
语义信息作为环境的高层次特征信息，不会受到视角变化的影响，为了更加符合人类感知环境的过程，系统通过语义类别将三维点云分为前景点和背景点，前景点包括杆状物、树木、信号指示牌等具有清晰的外观轮廓和指示意义的物体三维点云，背景点则为建筑物、灌木丛、草坪、地面等大规模三维点云，虽然背景点不会特别受到关注，却是表达环境不可缺少的部分；
[0080]
特征点周围近邻点有相同语义类别的三维点云越多，则该特征点的语义分数φ2(ph)越高，反之则语义分数越低，如式(5)所示：
[0081][0082]
其中，α表示系数因子，将特征点的几何分数φ1(ph)和语义分数φ2(ph)相乘得到特征点总分数φ(ph)，如式(6)所示：
[0083]
φ(ph)＝φ1(ph)*φ2(ph)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(6)
[0084]
所述构建语义拓扑图，采用鸟瞰图，以观测中心为圆心，半径由近到远等距的划分nr个同心圆，并且以观测前方作为起始角度，顺时针方向等间隔的径向划分ns个区域，同心圆划分线和径向区域划分线组合，将三维点云空间以从上向下的俯视角度将场景三维点云划分为多个栅格，将同一栅格中不同高度的特征点，通过局部极大值抑制选取最大分数的特征点作为所在栅格的代表点，所有栅格中的代表点作为语义拓扑图的节点；考虑特征点的空间距离，以栅格作为距离的基本单位，将任意两个栅格距离与特征点总分数相乘得到语义拓扑图的节点之间带有几何和语义权重的语义距离，取节点之间最大的语义距离作为图的边，构建出语义拓扑图；
[0085]
设定任意栅格的特征点为n
i,m
，其中i是栅格圈数的序号，i∈{1,2,
…
,nr}，nr为栅格圈数的最大序号；j表示栅格角度划分的第j序号，j∈{1,2,
…
,ns}，ns为栅格角度划分的最大序号；m表示栅格角度划分的第m序号，且m≠j；栅格特征点的语义类别为o，设定max为使得语义距离最大的函数，语义几何距离dg和语义种类距离ds计算如式(7)、式(8)所示：
[0086][0087][0088]
其中，o表示栅格特征点的语义类别；
[0089]
s4:将语义拓扑图转换为矩阵形式的描述符，如图5、图6所示，通过两阶段搜索策略，完成闭环检测；
[0090]
所述s4中，以栅格图径向角度作为起点，拉伸为二维矩阵，以语义距离作为矩阵中对应行列的数值，构建为矩阵形式的描述符
[0091]
由于机器人在行进过程中会存储大量的场景数据，因此使用暴力搜索进行闭环检测是不合理的，采用由粗到精的两步搜索算法，在保证精度的同时大幅度减少计算成本；
[0092]
所述两步搜索算法，包括第一步搜索算法及第二步搜索算法；以观测中心为圆心
的栅格地图由多个同心圆组成，径向间隔相同的角度进行划分，拉伸为首尾相接的二维矩阵；
[0093]
所述第一步搜索算法，将矩阵转换为向量k，采用k-d树(k-demension tree)的结构存储和搜索当前场景的候选帧；
[0094]
所述采用k-d树(k-demension tree)的结构存储和搜索当前场景的候选帧，包括：向量k的第一个值从矩阵的第一行获得，后面的值依次从下一行获得，设定no为所选语义种类的数量，nr为栅格划分圈数，为语义种类相同的栅格特征点的ds平均值，为圈数序号相同的栅格特征点的dg平均值，所有行在编码后生成nr+no维向量ε，如式(9)所示：
[0095][0096]
其中，r表示矩阵的行数，o是语义类别，
[0097]
所述第二步搜索算法，设定当前场景的矩阵描述符为sqg，栅格圈数为ni的特征向量为sqni，||sqni||代表其二范数，语义种类为sql，候选场景的矩阵描述符为scg，栅格圈数为ni的特征向量为scni，||sqni||代表其二范数，语义种类为scl，矩阵描述符中每个单位中存储的值为语义距离，使用余弦距离计算描述符之间的差异性，并且添加了语义信息的约束，d(sqg,scg)为矩阵描述符之间的相似度，计算公式如式(10)所示：
[0098][0099]
其中，为矩阵描述符每一行的相似度计算，如式(11)所示：
[0100][0101]
将计算得到的相似度和设定相似度阈值相比较，若相似度小于设定的相似度阈值，则当前场景存在对应的闭环场景，反之则当前场景不存在对应的闭环场景，闭环检测部分完成，将结果作用于同时定位和建图的过程中。
[0102]
为进一步验证本发明采用的技术方案能够取得很好的匹配精度，本实施例在室外数据集kitti上进行测试，在kitti00、02、05、06、07、08序列中存在大量的闭环场景，本实施例分别取得了0.984、0.843、0.970、0.991、0.915、0.897的f1最大分数，此外在具有动态物体的闭环场景中，本实施例的f1分数也保持在0.8以上。为了验证通过本方案计算得到的闭环场景漂移误差更小，在kitti00、02、05、06、07、08数据集上进行测试，其在偏航角上的误差分别为0.226、1.083、0.492、0.070、0.199、1.204、0.546，单位为弧度。
[0103]
表1.室外数据集kitti测试数据表
[0104][0105]
本实施例对计算耗时进行了实验测试，对于每一帧三维点云数据，平均的语义分
割模块耗时为83ms，描述符计算耗时为18ms，闭环场景匹配耗时为45ms，能够满足实际系统的耗时要求。