论文笔记—Voxelized GICP for Fast and Accurate 3D Point Cloud Registration

论文笔记—Voxelized GICP for Fast and Accurate 3D Point Cloud Registration

文章摘要

     ~~~~          ~~~    本文提出了体素化广义迭代最近点（VGICP）算法，用于快速准确的三维点云配准。所提出的方法通过体素化扩展了广义迭代最近点 (GICP) 方法，以避免代价高昂的最近邻搜索，同时保持其准确性。与从点位置计算体素分布的正态分布变换 (NDT) 相比，我们通过聚合体素中每个点的分布来估计体素分布。体素化方法使我们能够有效地并行处理优化，并且所提出的算法可以在 CPU 上以 30 Hz 和 GPU 上以 120 Hz 的频率运行。通过在模拟和真实环境中的评估，我们确认所提出算法的准确性与 GICP 相当，但比现有方法快得多。这将使实时 3D LIDAR 应用程序的开发成为可能，这些应用程序需要对 LIDAR 帧之间的相对位姿进行极快的评估。

关键词

点云、配准、GPU 计算

导语

     ~~~~          ~~~    三维 (3D) 点云的配准是许多 3D LIDAR 应用的关键任务，例如校准、定位、建图和环境识别。 3D LIDAR 有两种流行的点云配准方法：广义迭代最近点 (GICP) [8] 和正态分布变换 (NDT) [2, 4]。 GICP 以分布到分布比较的方式扩展了经典的 ICP 算法 [15] 以实现准确配准，而 NDT 则利用体素化方法来避免代价高昂的最近邻搜索并提高处理速度。这两种方法都有其自身的弱点。由于 GICP 和其他 ICP 变体高度依赖于最近邻搜索，因此如果点数很大，有时很难在计算能力有限的计算机上实时运行它们。NDT 通常对体素分辨率的选择非常敏感。最佳体素分辨率取决于环境和传感器特性，如果我们不选择合适的分辨率，NDT的配准精度会急剧下降。
　　在本文中，我们提出了体素化 GICP（VGICP）算法，用于快速准确的 3D 点云配准。 体素化方法使所提出的算法能够高效地并行运行，我们的 VGICP 实现可以在 CPU 上以 30 Hz 和 GPU 上以 120 Hz 的频率处理包含 15,000 个点的点云。 通过聚合体素中所有点的分布（多点分布到单体素分布），我们可以稳健地估计体素的分布。 与从点位置估计体素分布的 NDT 相比，即使体素中的点很少，这种方法也能产生有效的体素分布，从而产生一种对体素分辨率变化具有鲁棒性的算法。 在模拟和真实环境中的评估表明，所提出的 VGICP 算法显示出与 GICP 相当的配准精度，并且在处理速度方面优于其他方法。
　　本文的贡献是三方面的。 首先，我们提出了一种多点分布聚合方法，可以从较少数量的点中稳健地估计体素的分布。 其次，我们提出了 VGICP 算法，它与 GICP 一样准确，但比现有方法快得多。 第三，实现可从公共存储库1 获得。 该存储库包含提议的 VGICP 的实现以及 GICP 的并行实现。

相关工作

A.GICP

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

B.NDT

     ~~~~          ~~~    NDT [2] 采用基于体素的关联方法，而不是精确的最近邻搜索。该算法首先将输入点云拆分为一组体素，并用正态分布拟合到每个体素中的点。然后，它通过找到最大化输入点在体素分布下的可能性的变换，将另一个点云与体素化的点云对齐。因为 NDT 避免了代价高昂的最近邻关联，所以它本质上比 ICP 变体算法快得多。 D2D-NDT（Distribution-to-Distribution NDT）[12]是NDT的扩展，它对源点云和目标点云进行体素化，并计算源体素和目标体素分布之间的距离。其比较方案类似于GICP，[5]表明D2D-NDT在准确性方面优于经典NDT。然而，NDT及其变体的准确性取决于体素大小的选择。为了获得 NDT 的最佳性能，我们需要根据传感器和环境属性仔细选择合适的体素大小。一些研究提出了使 NDT 对超参数变化具有鲁棒性的方法（例如，多分辨率 [13] 和三线性体素平滑 [4]）。然而，这些扩展对处理速度有负面影响。

C.基于特征的配准

     ~~~~          ~~~    基于特征的配准方法首先从输入点云中提取一些有代表性的特征，然后从特征对应关系中估计变换。已经提出了许多用于点云配准的特征，例如基本平面和边缘特征 [11,15]、快速点特征直方图 (FPFH) [6] 和方向直方图特征 (SHOT) [7]。由于这些特征能够稳健地找到点云之间的对应关系，因此基于特征的方法对初始位姿误差具有内在的鲁棒性（有些甚至不需要初始估计）。但是，由于基于特征的方法仅使用有限数量的特征（通常远小于输入点的数量），因此其准确性不如基于点的方法。因此，在典型用例中，在基于特征的方法之后执行基于点的精细配准。基于特征和基于点的配准方法是正交的，应该相互补充。

提出的方法

     ~~~~          ~~~    在本节中，我们首先解释 GICP 算法，然后以一对多分布对应的方式对其进行扩展以推导出我们的 VGICP 算法

A.GICP算法

     ~~~~          ~~~    我们考虑变换$\mathbf{T}$的估计，它将一组点 A = { a 0 , ⋅ ⋅ ⋅ , a N } \mathcal{A}=\lbrace a_0,··· , a_N\rbrace A={a0​,⋅⋅⋅,aN​}（源点云）与另一组点 B = { b 0 , ⋅ ⋅ ⋅ , b N } \mathcal{B}= \lbrace b_0,··· ,b_N\rbrace B={b0​,⋅⋅⋅,bN​}（ 目标点云）。 遵循经典的ICP算法，我们假设A和B之间的对应关系由最近邻搜索给出：$b_i=\mathbf{T}{a}_i$。 GICP 算法 [8] 将采样点的表面建模为高斯分布：$a_i\sim N({\hat{a}}_i,C_i^A),b_i\sim N({\hat{b}}_i,C_i^B)$。 然后，我们定义转换误差如下：

$d_i$ 的分布由高斯分布的再生特性给出：

GICP 算法找到使方程的对数似然最大化的变换$\mathbf{T}$。 (3) 如下：

每个点的协方差矩阵通常是从它的$k$个邻居中估计出来的（例如，$k=20$）。 按照 [8] 中的建议，每个协方差矩阵通过将其特征值替换为 $(1,1,ϵ)$ 来进行正则化。 这种正则化使 GICP 作为一个平面到平面的 ICP 工作。

B.体素化 GICP 算法

     ~~~~          ~~~    为了推导出体素化 GICP 算法，我们首先扩展方程(1)， 计算 $a_i$ 与其相邻点 { b j ∣ ∣ a i − b j ∣ ∣ < r } \lbrace b_j||a_i− b_j|| < r\rbrace {bj​∣∣ai​−bj​∣∣<r} 之间的距离，如下所示：

这个方程可以解释为平滑目标点分布。 然后，类似于方程(3)。 ${d_i}^{\prime }$ 的分布由下式给出

我们估计方程(7)的对数似然最大化的变换$\mathbf{T}$。如下：

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


其中 $N_i$ 是相邻点的数量。 等式 (11) 表明我们可以通过用 $a_i$ 周围的点 ($b_j$ 和 $C^j$) 的分布的平均值代替等式(5)中的 $bi$ 和 $C_i^B$ 来有效地计算目标函数。 并通过$N_i$对函数进行加权。 我们可以通过在每个体素中存储 ${b_i}^{\prime }=\frac{\sum b_j}{N_i}$ 和 ${C_i}^{\prime }=\frac{\sum C_j^B}{N_i}$ 来自然地将该方程应用于基于体素的计算。
　　图 1 说明了 GICP、NDT 和我们的 VGICP 中使用的对应模型。 GICP 采用最近分布到分布对应模型，这是合理的，但依赖于代价高昂的最近邻搜索。 为了快速配准，NDT 使用点到体素分布对应模型。 但是，我们至少需要四个点（实际上超过十个）来计算 3D 协方差矩阵。 如果体素中的点数较少，协方差矩阵就会损坏。 我们的 VGICP 利用体素对应中的单到多分布来处理只有少数点落在一个体素内的情况。 因为它根据点分布计算体素分布，所以即使体素只包含一个点，它也会产生适当的协方差矩阵。

执行

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

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