Incremental Segment-Based Localization in 3D Point Clouds解读

Incremental Segment-Based Localization in 3D Point Clouds解读

				                   基于增量分割的3D点云定位
	本文转自于【泡泡机器人SLAM】微信公众号、https://www.sohu.com/a/272878455_715754

最近在读这篇文章，读完后没能形成系统性知识，刚好看到这篇文章，保存下来，希望可以帮到一些朋友（建议先看原文，原文链接）。
基于三维点云的定位是一件很有挑战性的工作，因为从三维数据中提取信息是一件非常复杂的工作。本文提出了一种能够有效解决这一问题的增量式方法。该方法首先将多次的测量以动态体素网格的形式累积，并有选择性地更新那些被新插入点影响的点的法向量。随后使用一种基于区域增长的增量式分割方法，它能够跟踪每个分割的更新，从而我们能够使用对几何一致性的分区和缓存进行有效的识别。本文表明该增量式的方法能够在城市驾驶环境中以10Hz进行全局定位，比所对比的方法提高了7.1倍。该方法的有效性使得其很适合实时定位场景，也使得其能够在更廉价的低功耗系统中使用。本文的实现和代码使用指导都是开源的。（本文的实现的网址https://github.com/ethz-asl/segmatch，视频示例网址https://youtu.be/cHfs3HLzc2Y）。

本文主要实现了一种基于三维点云的实时定位系统，其主要的贡献为：
1、提出了一种基于分割匹配的三维点云增量式定位方法；
2、一系列用于法向量估计，分割和识别的增量式方法；
3、通过灾难响应和城市驾驶实验，对增量方法与批量解决方案进行了详尽的比较。

本文的基于三维点云的实时定位算法的主要思路是：将多次测量的结果合成一个局部地图，对局部地图进行分割，而后对每个分割进行特征描述，并在特征空间对局部地图和目标地图进匹配，得到每个分割的候选对应关系，最后通过几何一致性验证，得到最终的定位结果。由于局部地图随着测量数据的添加不断在更新，为提高实时性，本文在局部地图维护、点的法向量计算、局部地图分割环节设计了增量式的计算方法，下面将对这些方法逐个进行简要的介绍：

1）动态体素网格
局部地图是以体素的形式存储，每个体素存储了其包含的点，以及这些点的质心。当得到新的测量（一些三维点），对这些点进行排序，并计算每个点的体素索引，计算方法为：

而后将这些点插入到对应的体素网格中，并增量式更新每个网格的质心：

2）法向量和曲率增量式计算方法
为增量式计算法向量，本文提出了两种准则：增量式计算协方差矩阵和只更新新添加点影响的点的法向量。协方差矩阵可表达为：

因此其增量式计算方法：

3）增量式区域生长分割算法
该算法如下：
其中CANGROWTO准则控制一个点是否属于某一分割，CANBESEED准则控制一个点能够成为新的分割的起点。准则的具体内容，请参考原文。

4）基于图的增量式识别算法
在得到局部地图的分割后，对每个分割进行特征描述，并在特征空间对局部地图和目标地图以分割为单位进行匹配，得到初始的分割对应关系，而后通过分割间图的几何一致性，来最终确定位置。因此，本文将最终的识别问题转化为最大对偶一致集问题（MPCS）。
本文中，几何一致性的定义为：对应的两点组成的线段长度之差在容忍范围内。为了避免验证图中所有的两点进行几何一致性验证，作者提出了对每个点（对应关系中的点）增量式维护其几何一致性候选点集，即：

该过程可由图1来描述：

1. 几何一致性缓存维护示例。

对应间具有任意的位置，他们之间的距离即为式5表达的几何一致性。箭头从一点指向其候选几何一致性点，圆表示缓存的阈值。（a）点c1，c2，c3以一定的顺序插入，c2被检测出为c1的几何一致性候选点（b）当点c4插入时，将c1，c2缓存为其候选点（c）分割c4的质心移动了小于1/2θΔ的距离，因此其缓存信息依然有效。C2移动了大于1/2θΔ的距离，因此其几何一致性候选需要重新计算。

主要结果
本文主要从两方面对算法的有效性，进行了验证。首先将本文的方法和基本的算法进行了比较，而后展示了一个仅有本文算法才能实现的很具挑战的大尺度多机器人SLAM应用。
1、与基本算法的比较
作为比较的基本算法是PLC库中实现的三维点云的定位算法。对比结果如下：
表1. 批处理和增量式方法不同模块的运行时间（MS）

表2. 不同实验的统计数据

图2. 移动某一距离没有成功定位的可能性（该数据通过运行5次KITTI数据集进行定位统计得到的）

上述结果表明，无论从定位的速率、定位的鲁棒性，本文的方法都优于传统的方法。

2、大尺度多机器人实验
为了模拟多机器人，将KITTI数据集序列0分割为5个序列，所有序列持续时间为114s。最终的实验结果如图3，

图3. 通过模拟五辆装有Velodyne HDL-64E传感器的自主车辆，在一台计算机上实时构建通用表示的自顶向下图解。分割的质心根据其关联的机器人着色。