点云回环位姿估计新强者！国防科大SGLC：实时完整闭环检测方法

最新推荐文章于 2024-08-18 13:43:10 发布

自动驾驶之心

最新推荐文章于 2024-08-18 13:43:10 发布

阅读量376

点赞数

原文链接：https://mp.weixin.qq.com/s?__biz=Mzg2NzUxNTU1OA==&mid=2247618902&idx=4&sn=1ead3fb85b1f80d2b4b6fef33ea005de&chksm=cfd912a78f5fc1ec08cdc4e011f16e698c41f6a5dde0ad4a0924b7771baabce03c51dd07a895&scene=126&sessionid=0

版权

点击下方卡片，关注“自动驾驶之心”公众号

戳我-> 领取自动驾驶近15个方向学习路线

>>点击进入→自动驾驶之心『SLAM』技术交流群

论文作者 | Huimin Lu

编辑 | 自动驾驶之心

闭环位姿估计精度不能丢

闭环检测在SLAM系统中能够纠正里程计漂移并构建一致的地图，尤其是在没有GPS信息的单传感器SLAM中。基于LiDAR的闭环检测通常需要从LiDAR扫描中提取特征以生成有区分度的描述子。主要分三大类：

基于手工制作的方法：这些方法通常利用点云中的几何特征，具有良好的描述能力。如NDT变换和Scan Context等方法经过多年的研究和改进，已经展示了较高的鲁棒性和实用性。一些方法通过将点云投影到2D平面或利用极坐标鸟瞰图等技巧，能快速生成和处理描述子。但是在处理复杂场景或动态环境时，这些方法的鲁棒性可能不够。固定的手工特征提取方法在应对不同类型的场景或任务时缺乏灵活性。
基于深度神经网络（DNN）的方法：利用深度神经网络，这些方法能够自动从数据中学习到有效的特征，表现出强大的特征提取能力。能够适应不同类型的场景和任务，具有较高的通用性。但是这些方法通常需要大量的计算资源，难以在实时系统中应用。一些方法如NetVALD生成的描述子维度较高，导致存储需求大，特别是对于低成本设备不友好。
基于语义信息的方法：结合语义信息，这些方法能更好地区分场景中的不同对象，增强了描述子的区分能力。随着语义分割技术的发展，语义信息在闭环检测中的应用前景广阔。但是结合语义信息的方法往往更为复杂，需要更多的计算资源和更复杂的算法设计。当前少数基于语义的方法能够有效地用于6-DoF闭环检测，仍需进一步研究和改进。

为了解决这些问题，本文介绍的SGLC[1]是一种高效的语义图引导的全闭环检测框架，提供了鲁棒的LCD和准确的6-DoF闭环姿态估计。与现有的语义图方法不同，有效地利用了前景实例和背景点云的不同特性。SGLC基于前景实例构建语义图，而在检索闭环候选和估计6-DoF姿态时，SGLC利用了语义图的拓扑特性和背景点的几何特性，增强了闭环检测的精度和鲁棒性。

SGLC首先生成LiDAR扫描描述子，通过利用前景语义图的语义和拓扑特性以及背景点的几何特性来快速检索多个候选扫描。为了防止错误闭环影响SLAM系统，应用几何验证来消除错误闭环候选，通过识别查询扫描和候选扫描之间的图节点对应关系。

基于语义图引导的闭环检测

如图2所示。SGLC包括以下几个部分：

从原始点云构建图
LiDAR扫描描述子生成
几何验证
位姿估计

语义图

语义图是SGLC方法的基础组件，生成独特的描述子并引导后续的几何验证和位姿估计。给定一个原始LiDAR扫描，可以使用现有的语义分割方法为每个点获得语义标签，从而区分移动和静止的物体。然后，将语义点云进行聚类，以识别物体实例。随后，通过包围这些聚类来估计实例的边界框。由于前景实例（如杆和树干）是天然独立的个体，因此它们可以轻松地表示为图中的单个节点。背景点云（如建筑物和围栏）通常具有大量点，且视角依赖性强，使其作为实例节点表示不稳定。因此，只为稳定的前景实例（如杆、树干、灯和静止车辆）构建语义图。每个节点v包含实例中心位置、边界框大小、语义标签和基于语义图拓扑生成的节点描述子。

如果两个节点的空间欧氏距离小于，则在它们之间建立图边。每个边4e = (v_i, v_j)由标签l_e和长度d描述。标签l_e包括杆灯、杆树干等类别。最终，我们获得了包含一组节点V和一组边E4的语义图G。图的邻接矩阵的生成如下：

其中，为节点数。

基于语义图，该方法设计了独特的实例节点描述子，供后续的节点匹配使用。首先，编码节点的局部关系，这些关系可以通过连接到节点自身的边捕获。因此，对连接到每个节点的所有边进行分类和量化，从而构建基于直方图的描述子。具体而言，对于节点，根据边的标签和长度将所有连接的边分类到不同的区间，并统计每个区间内的出现次数以生成描述子。

由于仅捕获了节点邻域内的局部拓扑，因此未考虑每个节点的全局属性（如它们在图中的全局中心性）。为了增强描述子的区分性以实现稳健的节点匹配，我们对图的邻接矩阵A进行特征值分解，得到。中的特征值λλ按降序排列。的第行表示第个实例节点的嵌入，捕获了节点在图中的全局属性。因此，使用的每一行作为实例节点描述子的一部分。考虑到每个图生成的矩阵的维度不同，并统一描述子的维度，使用的前k列，即与最大的k个特征值对应的特征向量，对每个节点进行编码，生成行向量f_g ∈ R^k。注意，特征向量的符号是任意的，因此我们使用它们的绝对值。最后，通过将f_l和f_g连接起来，得到每个实例节点在图中的描述子f ∈ R^D$。

用于检索的LiDAR扫描描述子

理论上可以使用局部节点描述子来确定图的相似性并直接找到闭环。然而，使用节点进行图匹配来实现扫描间的闭环检测是非常耗时的，不适合实时SLAM系统。为了快速检索闭环候选，设计了一种新颖的全局描述子用于每个LiDAR扫描，包括前景描述子和背景描述子。

基于所提出的前景语义图生成，利用图中的边和节点。F_f的第一部分类似于节点描述子的第一部分，即基于边的直方图描述子。不同的是，对于F_f$，考虑整个图中的所有边。这些边根据其类型和长度被分类到直方图的子区间中，形成全局边描述子。第二部分，统计语义图中具有不同标签的节点数量，以有效描述节点的分布。

除了捕捉前景语义图拓扑关系的外，进一步设计了一个描述子来利用背景点云中丰富的几何信息。受Scan Context启发，后者将原始点云的最大高度编码到不同的极坐标鸟瞰图图像单元中，本文利用语义信息代替高度数据，基于背景点云构建类似的描述子，以生成旋转不变的环形关键描述子。它有效地编码了不同背景点云的外观特征，以最小的计算成本提供简洁的描述。

最后对和进行归一化，并将它们连接起来，形成LiDAR扫描描述子。这种综合描述子结合了前景语义图和背景点的特征，具有更强的检索能力。LiDAR扫描描述子用于从数据库中检索多个相似的候选扫描。描述子之间的相似性通过特征空间中的欧几里得距离计算。

几何验证

一旦获得了闭环候选，就需要对每个候选进行几何验证，以确定是否发生了真实的闭环。几何验证的核心是建立节点对应关系并估计查询扫描和候选扫描之间的初始相对位姿。然后通过评估对齐程度来衡量它们的相似性，具体包括以下四个步骤：

实例节点匹配

对于具有语义图的查询扫描和具有语义图的候选扫描，根据节点描述子的相似性创建一个关联矩阵，其中和分别是查询图和候选图中的节点数。中的每个元素计算如下：

表示将中的第个节点分配给中的第个节点的成本。如果节点标签不一致或边界框大小差异显著，则分配一个高成本以拒绝匹配。

匹配结果通过匈牙利算法确定，得到和之间的实例节点匹配对应集：

其中是前面提到的节点位置，是节点匹配对的数量。

节点对应关系的修剪

由于重访时的视角变化或语义分割中的错误，存在不正确的节点对应关系。尽管RANSAC可以排除一些离群点，但在使用所有节点匹配进行操作时会显著增加迭代次数，不适用于在线应用。

为了降低计算复杂度，提出了一种基于节点局部结构的高效离群点修剪方案，在应用RANSAC之前进行。我们假设真实的正匹配对应节点应该表现出一致的局部几何结构。对于节点，其一定范围内的邻居节点可以表示为，其中是邻居总数。和其任意两个邻居可以形成一个三角形，表示为，4v的所有局部三角形表示为{Δ_j}_j=1^{K(K-1)/2}$。在理想情况下，匹配节点的对应局部几何三角形应完全对齐。然而，实际应用中会受到语义分割和聚类的误差影响。因此放宽这一条件，以防止排除正确的对应关系。在局部三角形匹配中，认为当对应边的长度相等时，两个三角形是一致的。通过这样做，可以有效地消除错误的节点对应关系。

初始位姿估计

基于修剪后的对应关系，表示为和，可以基于RANSAC和SVD分解高效地估计查询扫描和候选扫描之间的相对位姿。具体来说，在每次RANSAC迭代中，随机选择三对匹配对和，以基于SVD分解求解变换方程：

通过该过程，在每次迭代中得到一个变换，并最终从所有迭代中选择内点最多的变换作为闭环候选验证的初始位姿估计。

闭环候选验证

通过计算扫描相似性在两个层次上验证闭环候选。首先评估查询扫描和候选扫描之间的语义图相似性：

其中和是内点对应关系，是内点数量。衡量两个图的对齐程度。

此外，利用对齐查询扫描和候选扫描的背景点云，然后计算它们的背景描述子和的余弦相似性。相比仅使用图相似性，这一额外的项提供了更大的可靠性。如果和背景相似性都超过各自的阈值，则认为查询扫描和候选扫描是一个真实的闭环。随后我们估计它们的精确位姿变换。

6自由度位姿优化

前文提到的在稀疏实例节点级别上估计，可能不足以实现闭环。为了提高相对位姿估计的准确性，我们提出对输入语义点云中的所有前景实例点进行密集点配准，并使用作为初始变换。

借助已经获得的实例节点匹配，查找匹配实例之间的对应点变得简单快捷，因为搜索空间显著减少。实际上，已经初步对齐了查询扫描和候选扫描。因此，直接搜索最近邻点作为点匹配对，足以初始化并求解迭代最近点（ICP）：

其中是查询扫描和闭环扫描之间的实例点最近邻对应关系集。

密集配准结果涵盖了所有前景实例点的配准。然而，背景点（如建筑物和道路等提供稳定平面特征的点）对于位姿估计也非常有价值。因此，为了进一步优化密集点配准的位姿精度，利用点到平面的残差来细化最终的相对位姿。

其中是具有一致平面法向量的点对应关系集，表示为。

通过稀疏图节点匹配估计，通过密集实例点配准确定。这种从稀疏到密集的配准机制在粗到细的过程中提高了位姿估计的准确性。最后，利用背景点的点到平面约束进一步优化相对位姿。这种语义引导的粗-细-优化位姿估计过程考虑了实例点和背景点的对齐，通过分别处理实例点和背景点，每个对齐阶段都从一个有利的初始值开始，确保快速匹配和收敛，以及多步骤的稳健位姿估计。

实验效果

总结一下

SGLC是一种基于语义图引导的LiDAR闭环检测方法。首先为前景实例构建语义图，作为描述子生成的基础，并引导后续的几何验证和位姿估计。设计的LiDAR扫描描述子利用了语义图的拓扑特性和背景点云的外观特征，从而增强了闭环检测的鲁棒性。为位姿估计提出了粗细优化的配准方案，考虑了实例点和背景点的对齐，提供了高效且精确的结果。所提方法支持实时在线操作，便于集成到SLAM系统中，特别是语义SLAM。

参考

[1] SGLC: Semantic Graph-Guided Coarse-Fine-Refine Full Loop Closing for LiDAR SLAM

投稿作者为『自动驾驶之心知识星球』特邀嘉宾，欢迎加入交流！重磅，自动驾驶之心科研论文辅导来啦，申博、CCF系列、SCI、EI、毕业论文、比赛辅导等多个方向，欢迎联系我们！

① 全网独家视频课程

BEV感知、BEV模型部署、BEV目标跟踪、毫米波雷达视觉融合、多传感器标定、多传感器融合、多模态3D目标检测、车道线检测、轨迹预测、在线高精地图、世界模型、点云3D目标检测、目标跟踪、Occupancy、cuda与TensorRT模型部署、大模型与自动驾驶、Nerf、语义分割、自动驾驶仿真、传感器部署、决策规划、轨迹预测等多个方向学习视频（扫码即可学习）

网页端官网：www.zdjszx.com

② 国内首个自动驾驶学习社区

国内最大最专业，近3000人的交流社区，已得到大多数自动驾驶公司的认可！涉及30+自动驾驶技术栈学习路线，从0到一带你入门自动驾驶感知（2D/3D检测、语义分割、车道线、BEV感知、Occupancy、多传感器融合、多传感器标定、目标跟踪）、自动驾驶定位建图（SLAM、高精地图、局部在线地图）、自动驾驶规划控制/轨迹预测等领域技术方案、大模型、端到端等，更有行业动态和岗位发布！欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频