Abstract
在3D点云中定位因为从3D数据中提取信息的复杂度的原因是非常挑战的。我们提出了一个增量的方法来高效的解决这个问题。
- 首先会在动态的voxel积累观测，然后有选择性的更新点的normal。
- 一个incremental segmentation algorithm
我们展现说这个增量方案可以在10Hz的urban场景下进行全局定位，比起batch solution快了7.1倍。
1. Introduction
感知能力是很重要的。所以移动机器人经常会配备3D的time-of-filght的传感器来产生环境的精准重建。我们专注于3D点云的定位。可以在3D数据中做全局关联使得我们可以重建要给unified表达方式，不需要假设low drife，或者已知的相对起始点。
Contributions
- 基于segment matching的3D点云定位方法。
- 一组用来normal estimation, segmentation和recognition的增量算法。
- incremental approach和batch solution在urban驾驶下的比较
2. Related Work
a) Incremental point cloud segmentation
[3]提出了稠密点云分割的区域生长方法。 对每个输入点云，分割只会做一次，还有后续的融合步骤。这里只会考虑平面的分割，而我们的方案更泛化。【5】提出分割深度图。上述的所有方案都没有基于分割结果提出retrieving models的方案。
b) Efficient geometric verification
在stereo images的时候，有提出减少匹配数量的策略。【7】提出了RANSAC只能做空间一致性的检查。上述的方法都有图像的视差的假设，所有他们的精度受高disparity和视角强烈变化的影响。
3. Method
A. Dynamic Voxel Grid
持续的3D点云输入被filtered，然后再一个Local cloud里用voxel的方法累积。我们没有用每次有新观测的时候更新整个local cloud的batch voxel filtering方法，而是更新只有被新的点云影响的voxel。我们用DVG,一个高效的数据结构能够支持动态的插入和移除。
被占的voxel会被保存在一个vector里，每个voxel保存它的index，centroid和包含的点数。为了减少噪声，一个voxel会在有特定数量的点数的时候被认为是active。
1） Voxel Indexing
一个voxel就是一个大小是\(l\times w \times h\)， 每一个voxel有唯一的index（再区间\([0, l\cdot w \cdot h - 1]\)。 grid有一个固定的分辨率\(r\)，一个rigid T从是世界系到grid系 \(T_{mg}\)。为了计算的高效，我们要grid的大小是2的倍数，\(l=2^{l_{bits}}\), \(w=2^{w_{bits}}\), \(h=2^{h_{bits}}\)
2）Insertion and Removal
当新的点被插入的时候，DVG会计算它的indices，然后根据voxel id的增序排列。考虑到排列有\(\mathcal{O}(n\log(n))\)的复杂度，所以用batch voxelization来排序就很重要。当\(m\)个点\(q_i\)被插入到要给voxel（centroid是\(p\)是从\(n\)个点降采样）的时候，我们有：
\(p \leftarrow\left(n \cdot p+\sum_{i=1}^{m} q_{i}\right) \cdot \frac{1}{n+m}, \quad n \leftarrow n+m\)
3） Rigid Transformation
当回环检测的时候，。。。
B. Incremental Normal and Curvature estimation
一个点\(p_i\)在3D点云里计算它的normal的时候，通常是用neighborhood点集\(\mathcal{N}(p_i)\)的协方差矩阵\(M\)来衡量的。在用fixed-radius Nearest Neighbors(NN)找到邻居后，\(M_{i}:=\overline{\left(\nu_{j}-\bar{\nu}\right)\left(\nu_{j}-\bar{\nu}\right)^{\top}}\)
normal的估计等于\(M_i\)的normalized eigenvector。curvature是\(\sigma=\lambda_{0}\left(\lambda_{0}+\lambda_{1}+\lambda_{2}\right)^{-1}\)，这里\(\lambda_{0}<\lambda_{1}<\lambda_{2}\)是\(M_i\)的eigenvalues。
这里我们做了两个主要的优化步骤。协方差矩阵\(M_i\)是增量算的，并且只有被新扫到的点影响的normal才会被更新。
1）Incremental Updates 重看一遍
2） Rigid Transform
C. Incremental region growing segmentation
region growing policies 区域生长策略
1）Clusters merging:
2）Growing policies
在准备阶段，PREPARESEEDS会搜集通过CANBESEED测试的点的id，然后把他们通过curvature的增序排列。这样保证区域生长的起始点是最平坦的点，减少segments的数量。
CANGROWTO会返回true，如果seed的normal和邻居点是平行的。因为normals的旋转未知，这个大概通过点乘来确认。另外一个max阈值是点的curvature，为了通过CANBESEED测试。
欧氏距离policies很容易理解，因为增量的区域生长已经基于欧氏距离找到邻居candidate。所以，CANGROWTO和CANBESEED总会返回true，PREPARESEEDS只是搜集还没有被assign的点的id。
3） Segment Tracking
cluster ID仅仅是临时的值来定义点归属于同样的cluster，segment ID是lingtime-long的定义。segmentation的阶段能够让我们鲁棒的跟踪segments和他们在局部地图的持续的views，这也会带来很多好处。
在【1】中，多views不能和同一个segment联系，也会获得不同的IDs，从而导致目标地图的insertion of segment duplicates。
D. Graph-based incremental recognition
从局部点云中提取的segments会被一个generic feature vector描述（【13】中eigenvalue-based descriptor)。局部和target map的segments的候选匹配就会被通过NN搜索找到。如果一个对\(c_i, c_j\)的segment centorids的欧式举例少于一个阈值，那么就是几何一致的。在我们的方案中，我们用graph problem来描述recognition的问题，来定义个Maximum Pairwise Consistent Set (MPCS).
我们也利用了章节3-C.3的segment tracking。
\(\mathcal{S}\left(c_{i}\right)=\left\{c_{j} \in V | j \leq i \wedge \Delta\left(c_{i}, c_{j}\right) \leq \theta_{\Delta}+\epsilon\right\}\)
1）Cache Maintenance
如果一个匹配\(c_i\)第一次被找到，\(\mathcal{S}(c_i)\)就会被计算保存。如果这个匹配没有再被观测，那么就会被删掉。
2）Consistent candidates set identification
为了让两个匹配一致，他们的target segments的举例必须小于等于local map的直径。
3）Consistency Graph Construction
4）MPCS Identification
当一个MPCS的大小大于一个阈值的时候，我们认为一个recognition是成功的。
4. Experiments
A. Baseline
用来比较的baseline是原版的SegMatch[1]，是用标注的PCL库组成的。batch voxel filtering是由pcl::VoxelGrid，而batch normals estimation是用pcl::NormalEstimation. Batch Segmentation使用pcl::EuclidianClusterExtraction和pcl::RegionGrowing. Recognition是用pcl::GeometricConsistencyGrouping.
B. Performance
1）硬件： 所有实验都是再32GB的RAM,Intel i7-6700K上完成的。RAM的使用从来没有超过1.6G.
。。。
5. Conclusions
我们突出了3D点云定位的incremental的方案。跟之前的不同，这个方案会维护一个segmented local map，然后用增量的几何验证。这个加速可以达到10Hz，可以支持实时。
