0. 简介

之前我们在以前的文章中介绍了很多有关于点云匹配相关的知识，最近两年处理GICP这一大一统的ICP匹配方法以外，还有一个工作对体素化和ICP这两者打起了心思，《Voxelized GICP for Fast and Accurate 3D Point Cloud Registration》提出了一种体素化的广义迭代最近点（VGICP）算法，用于快速、准确地进行三维点云配准。该方法扩展了广义迭代最近点（GICP）方法的体素化，避免了代价昂贵的最近邻搜索，同时保持了算法的精度。与从点位置计算体素分布的正态分布变换（NDT）不同，我们通过聚集体素中每个点的分布来估计体素分布。体素化方法使算法能够高效地并行处理优化问题，所提出的算法在CPU上可以运行30hz，在GPU上可以运行120hz。通过在模拟环境和真实环境中的评估，我们证实了该算法的精度可以与GICP相媲美，但比现有的方法快得多。结合类ICP和NDT的两者的优点。

 这里深蓝AI的文章中也证明了这一点。

 值得一提的是与GICP一样，VGICP也是开源的，开源地址为：https://github.com/SMRT-AIST/fast_gicp.git

1. 文章贡献

三维点云配准是标定、定位、建图和环境重建的等任务中的关键任务。有两种主流的点云配准方法: 广义迭代最近邻方法GICP和正态分布变换NDT方法。GICP算法扩展了经典的ICP算法，通过计算分布到分布的形式提高了配准精度。NDT利用体素化方法避免高昂的最近邻搜索，提高处理速度。由于GICP和其他ICP算法的变种均依赖于最近邻搜索，这使得很难在计算资源受限的计算机中实时的处理大量点云数据。而NDT通常对体素的分辨率大小非常敏感。最佳的体素分辨率取决于环境和传感器属性，如果选择不当的分辨率，则NDT的精度将大幅降低。本文的通过聚合每个体素内所有点的分布，使得体素化的过程更为鲁棒。相比于NDT从点的空间位置估计体素的分布，本文的体素化方法即使体素中有很少的点，也能够产生有效的体素分布。这也使得算法对体素分辨率的改变更加鲁棒。VGICP论文内容写得还是比较详细充实的，从作者的归纳来看论文的贡献有三个方面：

1. 首先，提出了一种多点分布聚合方法来从较少的点稳健估计体素的分布。
2. 其次，提出了VGICP算法，它与GICP一样精确，但比现有方法快得多。
3. 第三，代码开源，并且代码实现了包含了所提出的VGICP以及GICP。 开源代码中包含有以下几个部分，同时支持ROS

FastGICP: multi-threaded GICP algorithm (~40FPS)

FastGICPSingleThread: GICP algorithm optimized for single-threading (~15FPS)

FastVGICP: multi-threaded and voxelized GICP algorithm (~70FPS)

FastVGICPCuda: CUDA-optimized voxelized GICP algorithm (~120FPS)

2. 具体算法

由于VGICP是由ICP变化而来，这里我们一一来将GICP和VGICP公式之间的对应进行梳理，以方便各位来进行对比：

2.1 初始化

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

为了能够推到出VGICP算法，我们首先对式1进行扩展，使得其可以计算点到其邻近点集 $\left\{b_{j} \mid\left\|a_{i}-b_{j}\right\|<r\right\}$ 的距离, 如下

$\hat{d}_i^{(T)}=\sum_j(b_j-Ta_i)$

2.2 高斯分布

$d_i^{(T)}$ 的分布由高斯分布的再生特性给出：

$\begin{aligned}d_i^{(T)} &\sim \mathcal{N}(\hat{b_i},C_i^{B}) - T \mathcal{N}(\hat{a_i},C_i^{A}) \\ &= \mathcal{N}(\hat{b_i}-T\hat{a_i},C_i^{B}+(T)C_i^{A}(T)^T)\\&= \mathcal{N}(0,C_i^{B}+(T)C_i^{A}(T)^T)\end{aligned}$

在VGICP中，这个方程可以解释为平滑目标点分布。类似于 $d_i^{(T)}$

$\begin{aligned}\hat{d}_i^{(T)} &\sim \mathcal{N}({\mu}^{d_i},C^{d_i})\end{aligned}$ $\begin{aligned} {\mu}^{d_i} = \sum_j(\hat{b}_j-\bold T\hat{a}_i) =0\end{aligned}$ $\begin{aligned}C^{d_i} = \sum_j(C^{B}_j+\bold TC_i^A \bold T^T)\end{aligned}$

′ 的分布由下式给出

2.3 最大化似然概率

GICP 算法找到使方程的对数似然最大化的变换 $\bold{T}$ $\begin{aligned}T&=\mathop{\arg\max}\limits_\bold{T}\prod_ip(d_i^{T})\\&= \mathop{\arg\max}\limits_\bold{T} \sum\limits_{i}\log (p(d_i^{(\bold{T})}))\end{aligned}$

估计体素化方程的对数似然最大化的变换 $\bold{T}$ 如下 $\begin{aligned}T&=\mathop{\arg\max}\limits_\bold{T} \sum_i (\sum_j(b_j-\bold{T}a_i))^T (\sum_j(C^B_j-\bold{T}C^A_i\bold{T}^T))^{-1}(\sum_j(b_j-\bold{T}a_i))\end{aligned}$

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

$\begin{aligned}T&=\mathop{\arg\max}\limits_\bold{T} \sum_i N_i(\sum_j(\frac{b_j}{N_i}-\bold{T}a_i))^T (\sum_j(frac{C^B_j}{N_i}-\bold{T}C^A_i\bold{T}^T))^{-1}(\sum_j(\frac{b_j}{N_i}-\bold{T}a_i))\end{aligned}$

其中 $N_i$ 为邻点的数量。 上式 表明我们可以通过用 $a_i$ 周围的点 ( $b_j$ 和 $C^j$ ) 的分布的平均值代替GICP等式中的 $b_i$ 和 $C\_i^B$ 来有效地计算目标函数。 并通过 $N_i$ 对函数进行加权。 我们可以通过在每个体素中存储 ${b_i}'=\frac{\sum b_j} {N_i}$ 和 ${C_i}'=\frac{\sum C_j^B}{N_i}$ 来自然地将该方程应用于基于体素的计算。

我们使用渐变的椭圆表示可能的分布，用实心的点表示确切的位置。不难看出，GICP实际上是一种 nearest distribution-to-distribution的方法，每次找到最近的点对，使用各自的局部几何性质加权距离损失，用以优化；而NDT是 voxel-based point-to-distribution 的方法，求解最优的变换，使得点投射到网格化的分布中时，达到的概率函数响应最大；VGICP则希望建模为voxel-based distribution-to-multidistribution方法，同时求解最有可能的分布到分布之间的配对关系。 相比较之下，NDT使用点对体素分布对应的模型，但至少需要4个点(实际多于10个)来计算一个立方体中的协方差矩阵，如果体素中的点数很低，协方差矩阵就不能完全可用，而VGICP根据点及其邻点的分布计算体素分布，所以即使体素仅包含一个点，它也可以生成适当的协方差矩阵。

3. 伪代码

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

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

4. 参考链接

https://blog.csdn.net/qq_46480130/article/details/117512879

https://mp.weixin.qq.com/s/fQHjYrFmvwhaWdAUU0DQAA

https://mp.weixin.qq.com

https://zhuanlan.zhihu.com/p/135454523