经典文献阅读之--VGICP（体素化的ICP匹配）
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论文内容写得还是比较详细充实的，从作者的归纳来看论文的贡献有三个方面：
- 首先，提出了一种多点分布聚合方法来从较少的点稳健估计体素的分布。
- 其次，提出了VGICP算法，它与GICP一样精确，但比现有方法快得多。
- 第三，代码开源，并且代码实现了包含了所提出的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 初始化
我们考虑变换 T \bold{T} 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 = T a i b_i= \bold T{a_i} bi=Tai。 GICP 算法 [8] 将采样点的表面建模为高斯分布： a i ∼ N ( a ^ i , C i A ) , b i ∼ N ( b ^ i , C i B ) a_i∼ N(\hat{a}_i, C_i^A), b_i∼ N(\hat{b}_i, C_i^B) ai∼N(a^i,CiA),bi∼N(b^i,CiB)。 然后，我们定义转换误差如下：
d i ( T ) = b i − T a i d_i^{(T)}=b_i-Ta_i di(T)=bi−Tai
为了能够推到出VGICP算法，我们首先对式1进行扩展，使得其可以计算点到其邻近点集 { b j ∣ ∥ a i − b j ∥ < r } \left\{b_{j} \mid\left\|a_{i}-b_{j}\right\|
d ^ i ( T ) = ∑ j ( b j − T a i ) \hat{d}_i^{(T)}=\sum_j(b_j-Ta_i) d^i(T)=j∑(bj−Tai)
2.2 高斯分布
d i ( T ) d_i^{(T)} di(T)的分布由高斯分布的再生特性给出：
d i ( T ) ∼ N ( b i ^ , C i B ) − T N ( a i ^ , C i A ) = N ( b i ^ − T a i ^ , C i B + ( T ) C i A ( T ) T ) = N ( 0 , C i B + ( T ) C i A ( T ) 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} di(T)∼N(bi^,CiB)−TN(ai^,CiA)=N(bi^−Tai^,CiB+(T)CiA(T)T)=N(0,CiB+(T)CiA(T)T)
在VGICP中，这个方程可以解释为平滑目标点分布。类似于 d i ( T ) d_i^{(T)} di(T)
d ^ i ( T ) ∼ N ( μ d i , C d i ) \begin{aligned}\hat{d}_i^{(T)} &\sim \mathcal{N}({\mu}^{d_i},C^{d_i})\end{aligned} d^i(T)∼N(μdi,Cdi)
μ d i = ∑ j ( b ^ j − T a ^ i ) = 0 \begin{aligned} {\mu}^{d_i} = \sum_j(\hat{b}_j-\bold T\hat{a}_i) =0\end{aligned} μdi=j∑(b^j−Ta^i)=0
C d i = ∑ j ( C j B + T C i A T T ) \begin{aligned}C^{d_i} = \sum_j(C^{B}_j+\bold TC_i^A \bold T^T)\end{aligned} Cdi=j∑(CjB+TCiATT)
′
的分布由下式给出
2.3 最大化似然概率
GICP 算法找到使方程的对数似然最大化的变换 T \bold{T} T
T = arg max T ∏ i p ( d i T ) = arg max T ∑ i log ( p ( d i ( 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} T=Targmaxi∏p(diT)=Targmaxi∑log(p(di(T)))
估计体素化方程的对数似然最大化的变换 T \bold{T} T如下
T = arg max T ∑ i ( ∑ j ( b j − T a i ) ) T ( ∑ j ( C j B − T C i A T T ) ) − 1 ( ∑ j ( b j − T a i ) ) \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} T=Targmaxi∑(j∑(bj−Tai))T(j∑(CjB−TCiATT))−1(j∑(bj−Tai))
为了有效地计算上述等式，我们将其修改为
T = arg max T ∑ i N i ( ∑ j ( b j N i − T a i ) ) T ( ∑ j ( f r a c C j B N i − T C i A T T ) ) − 1 ( ∑ j ( b j N i − T a i ) ) \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} T=Targmaxi∑Ni(j∑(Nibj−Tai))T(j∑(fracCjBNi−TCiATT))−1(j∑(Nibj−Tai))
其中 N i N_i Ni为邻点的数量。 上式 表明我们可以通过用 a i a_i ai周围的点 ( b j b_j bj 和 C j C^j Cj) 的分布的平均值代替GICP等式中的 b i b_i bi和 C _ i B C\_i^B C_iB 来有效地计算目标函数。 并通过 N i N_i Ni对函数进行加权。 我们可以通过在每个体素中存储 b i ′ = ∑ b j N i {b_i}'=\frac{\sum b_j} {N_i} bi′=Ni∑bj和 C i ′ = ∑ C j B N i {C_i}'=\frac{\sum C_j^B}{N_i} Ci′=Ni∑CjB来自然地将该方程应用于基于体素的计算。