原创 | 文 BFT机器人

01

摘要

本文提出了体素化广义迭代最近点（VGICP）算法，用于快速准确的三维点云配准。所提出的方法通过体素化扩展了广义迭代最近点（GICP）方法，以避免昂贵的最近邻搜索，同时保持其准确性。与从点位置计算体素分布的正态分布变换（NDT）相反，我们通过聚合体素中每个点的分布来估计体素分布。

体素化方法使我们能够高效地并行处理优化，并且所提出的算法可以在CPU上以30 Hz运行，在GPU上以120 Hz运行。通过在模拟和真实环境中的评估，我们确认所提出的算法的准确性与GICP相当，但比现有方法要快得多。这将使实时3D LIDAR应用程序的开发成为可能，这些应用程序需要对LIDAR帧之间的相对位姿进行极快速的评估。

02

相关介绍

三维 (3D) 点云配准对于许多3D LIDAR应用（例如校准、定位、测绘和环境识别）来说是一项至关重要的任务。3D LIDAR有两种流行的点云配准方法：广义迭代最近点 (GICP) 和正态分布变换 (NDT)。

GICP以分布到分布的比较方式扩展了经典ICP算法[4]，以实现准确配准，而NDT利用体素化方法来避免昂贵的最近邻搜索并提高处理速度。然而，这两种方法都有其自身的弱点。由于GICP和其他ICP变体高度依赖最近邻搜索，因此如果点数很大，有时很难在计算能力有限的计算机上实时运行它们。相反，NDT通常对体素分辨率的选择非常敏感。最佳体素分辨率取决于环境和传感器属性，如果我们不选择合适的分辨率，NDT的配准精度会急剧下降。

在本文中，我们提出了V oxelized GICP (VGICP) 算法，用于快速准确的3D点云配准。体素化方法使所提出的算法能够有效地并行运行，并且我们的VGICP实现可以在CPU上以30 Hz频率和GPU上以120 Hz频率处理包含15,000个点的点云。通过聚合体素中所有点的分布（多点分布到单体素分布），我们估计体素稳健。与从点位置估计体素分布的NDT相比，即使体素中只有很少的点，这种方法也会产生有效的体素分布，从而产生对体素分辨率变化具有鲁棒性的算法。在模拟和真实环境中获得的评估结果表明，所提出的VGICP算法的配准精度与GICP相当，并且在处理速度方面优于其他方法。

本文的贡献有三个方面。首先，我们提出了一种多点分布聚合方法，可以从较少数量的点稳健地估计体素的分布。其次，我们提出了VGICP算法，该算法与GICP一样准确，但比现有方法快得多。第三，可以从公共存储库获取这些实现，该存储库包含拟议的VGICP的实现以及GICP的并行实现。

03

相关工作

A.GICP

经典ICP算法有很多变体，例如Trimmed ICP[5]和Normal ICP[6]。GICP[1]是最流行的ICP变体之一。GICP以分布到分布匹配的方式扩展了经典ICP算法。尽管该算法以其良好的准确性而闻名，但该算法（以及其他ICP变体）高度依赖最近邻搜索来关联最近的点。虽然通常使用基于KD树的高效搜索，但最近邻搜索常常成为瓶颈，使得当点数较多时算法无法实时运行。此外，基于最近邻搜索的方法不适合在GPU上进行优化，因为它大量使用条件分支，这会显着降低GPU的性能。

B.NDT

NDT[2]采用基于体素的关联方法，而不是精确的最近邻搜索。该算法首先将输入点云分割成一组体素，并对每个体素中的点拟合正态分布。然后，它通过找到使体素分布下输入点的可能性最大化的变换，将另一个点云与体素化的点云对齐。由于NDT 避免了成本高昂的最近邻关联，因此它通常比ICP变体算法快得多。D2D-NDT（分布到分布无损检测）[7]是对源点云和目标点云进行体素化并计算源体素和目标体素分布之间的距离的NDT。其比较方案与GICP类似，[8]表明D2D-NDT在精度方面优于经典NDT。

然而，NDT及其变体的准确性取决于体素尺寸的选择。为了获得NDT 的最佳性能，我们需要根据传感器和环境属性仔细选择合适的体素大小。一些研究提出了使NDT对超参数变化具有鲁棒性的方法（例如，多分辨率[9]和三线性体素平滑[3]）。然而，这些扩展对无损检测的处理速度有负面影响。

C.Feature-based registration

基于特征的配准方法首先从输入点云中提取一些代表性特征，然后根据特征对应估计变换。已经提出了许多用于点云配准的特征，例如基本平面和边缘特征[10]、[4]、快速点特征直方图（FPFH）[11]和方向直方图签名（SHOT）[12]。由于这些特征能够稳健地找到点云之间的对应关系，因此基于特征的方法通常对初始姿态误差具有鲁棒性（有些甚至不需要初始猜测）。

然而，由于基于特征的方法仅使用有限数量的特征（通常远小于输入点的数量），因此其精度比基于点的方法差。因此，在典型的使用案例中，在基于特征的方法之后执行基于点的精细配准。基于特征的配准方法和基于点的配准方法是正交的，它们应该被用来互补。

04

提出的算法

在本节中，我们首先解释GICP算法，然后以一对多分布对应方式扩展它以导出VGICP算法。

A.GICP algorithm

我们考虑变换T的估计，它将一组点A={a0,···,aN}（源点云）相对于另一组点B= {b0,···,bN}（目标点云）。遵循经典的ICP算法，我们假设A和B之间的对应关系是通过最近邻搜索给出的：bi=Tai。GICP算法[1]将采样点的表面建模为高斯分布：ai∼N（ˆai,CAi）,bi∼N（ˆbi,CBi）。然后，我们定义变换误差如下：

di的分布由高斯分布的再生性质给出：

GICP算法找到使方程（1）的对数似然最大化的变换T。如下：

每个点的协方差矩阵通常是根据其k个邻居估计的（例如，k=20）。按照[1]中的建议，通过用（1,1,c）替换其特征值来对每个协方差矩阵进行正则化。这种正则化使GICP作为平面到平面ICP工作。

B.Voxelized GICP algorithm

为了推导体素化GICP算法，我们首先扩展方程：计算ai与其相邻点{bj|kai−bjk<r} 之间的距离，如下所示：

该方程可以解释为平滑目标点分布。然后，类似于等式。d0i的分布由下式给出：

我们估计使方程的对数似然最大化的变换T。如下：

为了有效地计算上式，我们将其修改为：

图1：（a）GICP、（b）NDT和（c）VGICP中距离计算的对应模型。红色圆圈表示源点，蓝色圆圈表示目标点。（GICP：最近的分布到分布，NDT：基于体素的点到分布，VGICP：基于体素的分布到多分布。）即使体素仅包含几个，VGICP模型也会产生有效的分布点。

分布到分布对应模型，这是合理的，但依赖于昂贵的最近邻搜索。为了快速配准，NDT使用点到体素分布对应模型。然而，我们至少需要四个点（实际上超过十个）来计算3D协方差矩阵。如果体素中的点数较少，协方差矩阵就会损坏。我们的VGICP利用体素对应中的单到多分布来处理只有几个点落在体素内的情况。因为它根据点分布计算体素分布，所以即使体素仅包含一个点，它也会产生适当的协方差矩阵。

C.Implementation

算法1详细描述了VGICP的配准过程。如上所述，VGICP算法在优化过程中不需要昂贵的最近邻搜索，因此可以利用CPU和GPU并行处理。对于位姿优化，我们选择高斯-牛顿优化器，因为与拟牛顿方法不同，它收敛速度快并且不需要超参数。

我们实现了VGICP算法的三个版本：单线程、多线程和GPU处理。所有版本首先使用基于KD树的最近邻搜索来估计每个点的协方差矩阵[13]。这种协方差估计在多线程和GPU处理版本中是并行的。我们还实现了基于GPU的强力最近邻搜索。作为基线，除了VGICP之外，我们还实现了GICP算法。GICP实现也是通过CPU多线程并行化的，但没有在GPU上实现，因为它依赖于KD树最近邻搜索，不适合GPU。

05

总结及未来的工作

在本研究中，我们提出了体素化GICP算法。所提出的VGICP与GICP一样准确，因为它利用基于体素的关联方法。模拟和真实环境中的评估结果表明，所提出的方法显示出卓越的处理速度（CPU上为30 fps，GPU上为120 fps），并且对体素分辨率变化具有鲁棒性。我们计划评估和改进所提出的VGICP算法的收敛性，因为它采用体素化方法，当初始猜测不接近真实姿态时，可能会影响配准结果。我们还计划使用VGICP开发基于IMU-LIDAR联合优化的定位方法，这需要对关键帧之间的相对位姿进行极快的评估。

END

作者 | 江城

排版 | 小河

审核 | 橙橙

若您对该文章内容有任何疑问，请与我们联系，我们将及时回应。