segMatch:基于3D点云分割的回环检测

该论文的地址是：https://arxiv.org/pdf/1609.07720.pdf

segmatch是一个提供车辆的回环检测的技术，使用提取和匹配分割的三维激光点云技术。分割的例子可以在下面的图片中看到。

该技术是基于在车辆附近提取片段（例如车辆、树木和建筑物的部分），并将这些片段与从目标地图中提取的片段相匹配。分段匹配可以直接转化为精确的定位信息，从而实现精确的三维地图构造和定位。在先前记录的部分（白色）和最近观察到的部分（彩色）之间，匹配的段的实例用绿色线显示在下面的图像中。

该方法依赖于分割对象但不一定限于语义对象，因为这允许更一般的表示分割物，并在更广泛的不同环境中启用功能。

对三维点云数据进行回环检测一直都是一项挑战，但是已经有方案通过使用基于图像的方法解决回环检测的问题，基于图像的方法一般是根据图像的局部特征或者全局特征，图像中环境的变化会使得局部特征点会收到模糊性和鲁棒性的干扰，而基于全局特征点的方法一般的方法根据相机的视角（视点），该论文提出了一种可靠的闭环检测算法——SegMatch，是基于3D点云分割匹配的方法。Segment在局部描述和全局描述之间提供了很好的折衷方案，不仅融合了它们的优点，同时减少了各自的缺点。Segmatch方法不依赖完美分割（perfect segmentation）的假设，或者要求环境中必须存在的物体对象，它可以在大规模、非结构化环境下可靠良好的运行。

文章中结果表明，Segmatch可以在里程计数据集的最大序列频率为1Hz实现精确定位。并且，我们还展示了如何在在线运行过程中实现实时地检测和闭环检测。并且代码都是开源了：https://github.com/ethz-asl/segmatch（编译源码可能会遇到各种问题，有编译过的，遇到问题如何解决的还望大家一起交流分享一下）

在ＳＬＡＭ领域回环检测是一个重要的挑战，但是由于全局位姿信息的状态估计的漂移是不可避免的，所以对于机器人来说可靠的回环检测是十分重要的，当然前人也提出了很多典型的基于图像的闭环检测的方法，但是当环境发生强烈的照明变化，或者由于传感器的视点发生巨大的变化时，这种基于图像的回环检测的方法就显得不那么可靠了，基于激光雷达定位的方法，不受照明度的影响，并且雷达能够获取一定分辨率的几何信息，不会像视觉系统受视点的影响，这篇论文就是就是根据3D测距雷达传感器在室外环境中实现良好的稳定的定位。

对于3D点云数据的回环检测，当前的方案主要是依据关键点的检测以及匹配的方法

图1 此图是一个回环检测的框架图，参考点云在下面为白色部分，局部点云在对应的上方，不同的颜色代表着分割的结果，绿色的线代表着分割结果的匹配。

对于理想情况下的回环一般满足两个假设：第一是能够应用分割技术将物体分割出来，第二是环境中必须有物体对象，当环境中或者分割的条件不能满足这

两个假设时，就可以应用本文提出的Segment方法，该方法是利用整体点云中的部分或者整体的形状而不是使用关键特征点的方法，比如环境中的窗口，弓形等

结构，该Segment 系统是一种模块化设计。它首先第一步是从获得的3D点云中提取并描述分割物，将它们匹配用于已经走过的地方和使用几何验证对比的方法选出回环检测的候选点云。这种基于分割的技术的优点是将点云压缩成一组清晰的用于闭环检测的判别元素。实验表明这不仅减少了匹配所需的时间，也减少了错误匹配的可能性。

这是第一篇文章提出了在三维激光数据的基础上分割物体并执行闭环检测和定位的实时的算法。该文章的主要贡献有：Segmatch，是一种基于分割算法的三维点云进行位置识别的方法。开源代码实现实时的闭环检测，算法的性能在实际应用中得已检测。

目前主要三个趋势：（1）基于局部特征的方法；（2）基于全局的描述（3）以平面或物体为基础的

在论文中，描述了在三维点云中的位置识别方法。所提出的系统框架的具体如图2 所示，由四个不同的模块组成：点云分割、特征提取、分段匹配和几何验证。模块化一直是设计阶段的一个驱动因素。

图2

该图为SegMatch的框架结构图，其中Target Map可以是从本地磁盘中加载也可以是在线读取用于实现闭环检测

（1）Segmentation

SegMatch的第一步是根据3D点云进行不同的元素分割用于匹配。我们首先将输入的点云P进行体素网格化，体素化为了筛选出体素内没有占有太多的噪声。过滤后的点云被分割成一组点簇Ci，此分割的方法要求将点云数据中的地面去除，这可以通过基于垂直均值和方差的邻近体素聚类来实现。在论文[ 22 ]有所提及。一旦去除地面平面的数据，Euclidean聚类用于区域分割。对于每个集群Ci，质心ci被计算为其所有点的平均值。

(2) Feature extraction

根据第一步的分割结果 ，对于每一个分割结果进行特征提取，该特征提取的方法是压缩原始点云并为该点云生成对象签名，该签名适用于识别和分类，由于对3D点云数据来说没有十分清晰的黄金标准描述子，所以论文中使用了几个不同的描述子对聚类结果Ci进行描述，计算出描述子的特征向量Fi= f1 f2 ...fm,这些特征向量的表示可以扩展到包含很多的描述子，比如两个描述子的表示方式 ：

f1 表示基于特征值：在这个描述子中，计算出分割结果点云的特征值，结合一个1*7 维度的特征向量，该七个数据分别是linearity,planarity, scattering, omnivariance, anisotropy, eigenentropy 和 change of curvature measures

f2 表示形状直方图：此特征是一个1*640的维度组成的10个直方图，是形状函数D2，D3和A3的形状的解码表示，至于D2，D3 A3分别代表什么，在文章【6】

有更加具体的描述，D2形状函数是随机选择的点对之间的距离的直方图    D3是随机选择点对组成的三角形的面积   A3表示由D3随机组成的三角形的两两线段之间的角度函数

（3）Segment matching

使用第二步骤中获得的特性点，一般情况下，我们希望识别源点云和目标点云之间的匹配关系。对于这一步，论文使用学习 的方法，因为使用传统的方法通常很难选择合适的距离度量和阈值，特别是当涉及多个特征类型时。因此，分类器用于决定两个部分的匹配的点云是否代表相同的对象或对象中某一部分。为了保持效率，首先通过在特征空间中执行一个KD树搜索来检索候选匹配，然后再将其送入分类器。具体的说，论文中使用随机森林的分类和定时性能。这种分类器的思想是构造大量不同的决策树，并让它们为获胜类投票。在学习阶段，每一棵树都是利用训练数据集的特征及随机子集自举子集训练。随机森林提供了类似于AdaBoost算法的分类性能，但对输出标签中的噪声不敏感，因为它确实存在。随机森林还可以提供有关特征对于分类任务的相对重要性的信息。

对于随机森林分类器，用以确定Ci与Cj集群点云代表着相同的物体，计算的特征值的特征向量之间的绝对差：∆F =|  fi - fj  |。特征向量Fi和FJ放入的总特征值为1x21特征分类。这十个直方图分别是对形状特征的集合，直方图相交计算得到一个尺寸1*10的特征。考虑到这些特性，随机森林分类器分配一个分类分数w作为匹配。应用W上的阈值来构建传递给下一个模块的候选匹配的最终列表。

（4）Geometric verification

根据第（3）步得到的候选匹配被输送到几何验证模块。采用随机抽样一致性测试（RANSAC）[ 25 ]。使用分割物体的质心计算两物体之间的转换矩阵。几何一致的分割的簇点云最终被认为是同一物体是在其上的最小分割的基础上，得到的结果是6自由度变换和一系列的匹配的分割点云

增量分割

前面的部分展示了使用分割的技术匹配3D点云的方法。为了执行闭环检测，需要在线建立目标点云。对于在全局引用中给定的每个传入点云框架，这个模块首先提取局部点云。定义半径r，也就是当前机器人位置中心的圆柱邻域。分割和特征提取只执行一次，生成的源分割点云用于匹配和构建目标地图。所以在目标地图中添加源分割点云时，以下两个特殊情况需要处理：

分割的点云不完整的情况：将柱面滤波器应用于点云地图时必然导致物体对象的切割发生，从而导致点云“不完整分割”，从而干扰了目标地图中的完整视图。因此，这些“不完整的部分”点云被检测和丢弃，以便使得地图尽可能包含尽可能多的“完整段视图”。这可以通过一个较小的半径 r= R−Bd的源点云地图的滤波来实现，其中B是外点区域的厚度。在这个区域内有点云很可能代表不完整的分割物，因此可以安全地移除。

重复的分割点云：添加到目标地图中的分割物体也可能是之前分割的点云，也就是相同的对象部分，但在不同的时间分段。作为里程计是局部准确，这些重复的点云可以通过比较最近的距离来有效地检测分割物体的质心。希望保留最新的分割点云，并给出我们可以丢弃的不完整的分割部分，选择删除这些副本中最旧的分割部分。进一步的工作包括合并这些“重复片段”技术。

在一此闭环检测过程中，机器人的轨迹将会被重新估计，并且目标分割点云的位置将会被刷新，知道分割点云相对机器人轨迹的位置，在成功检测的条件下，目标地图中的分割点云将会被正确对其，也能够正确的滤波去除那些上文中提到的重复点云，所以滤波的作用就是从最近的分割执行到以前的分割点云。滤除掉重复的部分。

对于提取源点云的半径圆柱邻域的R设置为60米。体素网格叶的大小设置为0.1米，最小的两个体素内的点云将视为占用点。对于分割采用最大欧几里德距离的方法，两个被占据的体素使它们被认为属于将同一群集的阀值设置为0.2米。而且选择考虑仅包含最少有100个点的分割和最多15000点的分割物体。

Training and testing setup

执行该算法的过程来生成训练和测试数据集。在第一部分的给定序列中，通过提取和描述分割物来生成和处理目标地图。当车辆测量到环境中相同的一部分时，数据集用于存储对应不同分割物的源和目标点云。对于局部点云中的每个部分，我们在特征空间中执行KNN检索，并在目标地图中识别最近的200个邻居点。这些候选被保存为对应段的真实匹配和不同段的假匹配。使用此程序对该数据集的06个序列，我们产生2000真匹配，和800000假匹配。训练的随机森林时，我们采用1:50的阳性和阴性样品，结果有102000个样本训练集

Segment matching performance

表一                                                                                                       图3                                                                          图4

第一个实验的目的是评估三种分割匹配技术的性能，第一种方案是命名为L2表示一个欧几里得阀值，也就是两分割点云的特征向量之间的距离，第二种方案叫做

RF-eigen 是基于随机森林的方法主要依赖特征点的特征值，最后一种方法是RF_eigen+shapes 的方法，就是在第二种方案的基础上增加了几何信息，使用更多的特征就像上文提到的方法，每一种方案用于分类器中的总结如表所示

从00序列提取的数据中检测出三种方法的工作特性曲线（ROC）曲线。与它们的L2范数对应的相比，随机森林分类器提供了性能上的改进。相应的的rf_eigen +形状正确识别的例子是图4所示。

定位性能
这一部分对segmatch算法在目标地图中的定位的性能进行评估。数据集在序列00的部分是用于创建目标地图，定位是发生在再次经过该序列的数据集时。基于前文所述的三种比较的方法，说明以关键点为基础的回环检测技术。
1）关键点：关键点为基线的定位方法，首先计算各点的法线的过滤之后的点云，使用的滤波半径为0.3米。在这一章节使用到了PCL的库函数，关键点提取是在目标和源点云均使用PCL库的harris 3d关键点。这些关键点提取之后仍然进行滤波保留最少要有0.5米的距离一个关键点，确保同一地区没有两个太接近的描述子，从而减少几何验证阶段对于描述子的歧义性或者模糊性。每一个关键点是使用半径为0.4米的快速点特征直方图描述（fpfh）
。在进行源点云进行匹配是，寻找75个邻域在目标点云周围的分割点，并使用几何验证算法来过滤这个关键点的匹配列表和输出回环检测。为了获得我们能找到的最佳性能，需要我们决定最佳的参数，
2）结果：为了显示定位信息，我们对每个方案执行90次运行，并给出平均结果。每个局部点云之间的距离被记录并以类似于[ 28 ]的方式进行评估。并显示在目标地图没有成功定位的情况下行进给定距离的概率。具体来说，这个度量值计算如下：

图4  是成功检测到相应节段的segmatch算法。顶部和底部的行分别显示目标和源点云的分割点云。

那么对于增量分割的算法，解释如下

现在论文展示了基于分割的回环检测算法可以在线使用，也可以结合估计姿态图轨迹系统使用。在这个场景，如第四节所述，目标地图是可以在线构建的，应用这一策略方法针对数据集05数据集的结果，如图6所示。这一序列中，全局地图是添加Velodyne扫描之间约束使用迭代最近点（ICP）的方法创建的。在这个序列中，该实时算法非常成功地发现了12个真阳性和假阳性闭环检测。一次回环检测，他们的类似的描述在posegraph优化系统中比如论文[ 29 ]中就有详细的介绍 。此优化的结果用于更新目标分割物位置并从中删除重复分割物更新目标地图

基于对苏黎世的Clausiusstrasse数据平滑约束区域增长分割的闭环检测的插图

本文提出了segmatch，从基于段匹配的概念，三维激光数据检测闭合环的算法。相比于一个关键点的方法，在段水平提供了几个优点而不做任何关于完美的分割或环境中存在的“物体”的假设。我们的模块化方法首先从源点云中提取片段，然后将其描述和匹配到先前映射的目标段。一个几何验证步骤，最后把这些候选匹配到loopclosures。

该框架已在该数据详尽的评估。我们首先分析了使用随机森林分类器学习适当的距离度量的特征匹配段的目的的影响。我们表明，该算法能够准确地定位在一个频率高于1Hz的该数据集的最大的地图。我们还演示了如何健壮地检测在线方式的循环，以及如何将这些馈送到姿态图轨迹估计器。由于框架的模块化方法，我们进一步说明了它可以很容易地应用到不同的场景，通过简单地改变算法的构建块。整个框架的代码可在线获取，为三维点云流提供实时分割和环路闭包检测。
基于这种分段匹配技术，我们预见了在系统中不仅仅是匹配和描述环境的多个映射使用段的可能的优点。我们将采用有监督的学习技术，将这些基于段的映射解释为结构和对象语义类。

总结一下，这segmatch开发包提供了以下功能：

高效点云分割提供更为有信息的分割物
提供更为有效的点云描述子的提取
使用最近邻搜索的方法有效对分割段点云进行检索
基于随机森林分类器的鲁棒的进行分段点云匹配
结果：

三维点云的全局定位（达到闭环检测的作用）
实时性能
构建了基于激光雷达数据的地图构建和定位的ROS兼容代码库

分割点云：

首先，传入的点云被聚类成一系列的分割物。实际上，这可以用多种不同的方式来完成。在当前的实现中，分割首先通过去除地面平面，将像素网格应用到点云，然后过滤掉噪声。之后由欧几里德聚类形成分割聚类点云。

分割聚类点云的描述：

一旦点云已被分割成一系类的点云，为每个分割聚类点云提取描述子。此特征提取步骤用于将原始数据压缩成适合于识别和分类的紧凑描述符。在当前论文的实现中，既提供基于特征值的特征，又提供形状特征的集成。（那么关于基于特征值的特征以及基于形状的特征的提取的论文分别是：

（1）SEMANTIC 3D SCENE INTERPRETATION: A FRAMEWORK COMBINING OPTIMAL NEIGHBORHOOD SIZE SELECTION WITH RELEVANT FEATURES

（2）Ensemble of Shape Functions for 3D Object Classification

分段匹配：

给定查询的分段点云，接下来的目标是识别以前生成地图中分段点云的匹配的任务。这是通过首先在特征空间中使用KD树搜索检索相关分段点云，然后将每个检索片段分类为匹配或不匹配。由于选择合适的距离度量和阈值往往比较困难，因此对随机森林进行训练，并对其是否匹配进行分类。一旦分段点云匹配通过分类器的确定，再利用分段点云质心之间的对应的关系的几何一致性检查验证匹配度。如果场景几何一致，返回6自由度姿态，在便在地图提供的定位信息。

回环检测：

在这种情况下，现有的地图是不提供的，segmatch也可以用来识别闭合环和正确估计的漂移。这可以在在线（实时）或离线阶段进行。此外，因为segmatch执行全局搜索，闭环检测即使在大漂移的情况仍然是可能的。一个地图从该05序列产生一个例子如下所示。估计的轨迹显示在左边，检测到的闭合环用蓝线表示；在关闭循环闭包后进行图形优化的结果显示在右边。

下载数据集http://robotics.ethz.ch/segmatch/

rosbag首先会停止给程序一些时间从磁盘加载点云提取和描述部分。完成后，将从目标点云中看到白色部分，如下图所示。

一应用步骤教程 https://github.com/ethz-asl/segmatch/wiki/Demonstrations 

（个人理解，并且翻译水平有限，如有理解上的错误，欢迎指出，并期待你与我交流，联系方式：dianyunpcl@163.com）

REFERENCES

[1] S. Thrun et al., “Robotic mapping: A survey,” Exploring artificial intelligence in the new millennium, vol. 1, pp. 1–35, 2002.

[2] S. Lowry, N. Sunderhauf, P. Newman, J. J. Leonard, D. Cox,P. Corke, and M. J. Milford, “Visual place recognition: A survey,” IEEE Trans. on Robotics, 2016.

[3] M. Bosse and R. Zlot, “Place recognition using keypoint voting in large 3D lidar datasets,” in IEEE Int. Conf. on Robotics and Automation, 2013.

[4] B. Alexe, T. Deselaers, and V. Ferrari, “What is an object?” in IEEE Conf. on Computer Vision and Pattern Recognition, 2010.

[5] P. Scovanner, S. Ali, and M. Shah, “A 3-dimensional sift descriptor and its application to action recognition,” in ACM Int. Conf. on Multimedia, 2007.

[6] W. Wohlkinger and M. Vincze, “Ensemble of shape functions for 3d object classification,” in IEEE Int. Conf. on Robotics and Biomimetics, 2011. 

[7] R. B. Rusu, N. Blodow, and M. Beetz, “Fast point feature histograms (fpfh) for 3d registration,” in IEEE Int. Conf. on Robotics and Automation, 2009, pp. 3212–3217.

[8] Y. Zhuang, N. Jiang, H. Hu, and F. Yan, “3-d-laser-based scene measurement and place recognition for mobile robots in dynamic indoor environments,” IEEE Transactions on Instrumentation and Measurement, vol. 62, no. 2, pp. 438–450, 2013.

[9] B. Steder, G. Grisetti, and W. Burgard, “Robust place recognition for 3D range data based on point features,” in IEEE Int. Conf. on Robotics and Automation, 2010.

[10] B. Steder, M. Ruhnke, S. Grzonka, and W. Burgard, “Place recognition in 3d scans using a combination of bag of words and point feature based relative pose estimation,” in IEEE/RSJ

Int. Conf. on Intelligent Robots and Systems, 2011. 

[11] A. Gawel, T. Cieslewski, R. Dubé, M. Bosse, R. Siegwart, and J. Nieto, “Structure-based Vision-Laser Matching,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, Daejeon, 2016. 

[12] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in real-time,” in Robotics: Science and Systems, 2014.

[13] T. Rohling, J. Mack, and D. Schulz, “A fast histogrambased similarity measure for detecting loop closures in 3-d lidar data,” in IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 2015.

[14] K. Granström, T. B. Schön, J. I. Nieto, and F. T. Ramos, “Learning to close loops from range data,” The Int. Journal of Robotics Research, vol. 30, no. 14, pp. 1728–1754, 2011.

[15] M. Magnusson, H. Andreasson, A. Nüchter, and A. J. Lilienthal, “Automatic appearance-based loop detection from threedimensional laser data using the normal distributions transform,” Journal of Field Robotics, vol. 26, no. 11-12, pp. 892–914, 2009.

[16] E. Fernandez-Moral, W. Mayol-Cuevas, V. Arevalo, and J. Gonzalez-Jimenez, “Fast place recognition with plane-based maps,” in IEEE Int. Conf. on Robotics and Automation, 2013.

[17] E. Fernández-Moral, P. Rives, V. Arévalo, and J. GonzálezJiménez, “Scene structure registration for localization and mapping,” Robotics and Autonomous Systems, vol. 75, pp.

649–660, 2016. 

[18] R. Finman, L. Paull, and J. J. Leonard, “Toward object-based place recognition in dense rgb-d maps,” in ICRA workshop on visual place recognition in changing environments, 2015.

[19] B. Douillard, A. Quadros, P. Morton, J. P. Underwood, M. De Deuge, S. Hugosson, M. Hallström, and T. Bailey, “Scan segments matching for pairwise 3d alignment,” in IEEE

Int. Conf. on Robotics and Automation, 2012.
[20] J. Nieto, T. Bailey, and E. Nebot, “Scan-slam: Combining ekfslam and scan correlation.” Springer, 2006, pp. 167–178.

[21] B. Douillard, J. Underwood, V. Vlaskine, A. Quadros, and S. Singh, “A pipeline for the segmentation and classification of 3d point clouds,” in Experimental Robotics. Springer, 2014, pp. 585–600 

[22] B. Douillard, J. Underwood, N. Kuntz, V. Vlaskine, A. Quadros, P. Morton, and A. Frenkel, “On the segmentation of 3d lidar point clouds,” in IEEE Int. Conf. on Robotics and Automation, 2011.

[23] M. Weinmann, B. Jutzi, and C. Mallet, “Semantic 3d scene interpretation: a framework combining optimal neighborhood size selection with relevant features,” ISPRS Annals of the

Photogrammetry, Remote Sensing and Spatial Information Sciences, vol. 2, no. 3, p. 181, 2014.

[24] L. Breiman, “Random forests,” Machine Learning, vol. 45, no. 1, pp. 5–32, 2001. [25] M. A. Fischler and R. C. Bolles, “Random sample consensus: a paradigm for model fitting with applications to image 

analysis and automated cartography,” Communications of the ACM, vol. 24, no. 6, pp. 381–395, 1981.

[26] A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving? the kitti vision benchmark suite,” in IEEE Conf. on Computer Vision and Pattern Recognition, 2012. 

[27] R. B. Rusu and S. Cousins, “3D is here: Point Cloud Library (PCL),” in IEEE Int. Conf. on Robotics and Automation, 2011.

[28] C. Linegar, W. Churchill, and P. Newman, “Work smart, not hard: Recalling relevant experiences for vast-scale but timeconstrained localisation,” in IEEE Int. Conf. on Robotics and

Automation, 2015.

[29] R. Dubé, H. Sommer, A. Gawel, M. Bosse, and R. Siegwart, “Non-uniform sampling strategies for continuous correction based trajectory estimation,” in IEEE Int. Conf. on Roboticsand Automation, 2016.

[30] T. Rabbani, F. Van Den Heuvel, and G. Vosselmann, “Segmentation of point clouds using smoothness constraint,” International Archives of Photogrammetry, Remote Sensing andSpatial Information Sciences, vol. 36, no. 5, pp. 248–253,

2006.

有问题请指出，同时欢迎大家关注微信公众号

或者加入3D视觉微信群一起交流分享