点云配准各种方法总结[不定时更新]

　　本文主要用来记录我在学习过程中接触到的一些或新或旧的点云配准的方法，有些方法我可能还不怎么理解，如果有错误欢迎大家指出。

---

2018/04/15 添加LORAX
2018/04/08 添加DO法和结合法
2018/04/01 初版

---

　　我们都知道点云配准分为粗配准（Coarse Registration）和精配准（Fine Registration）两个阶段，本文同样也分为粗配准和精配准两个部分来说明。

粗配准方案

LORAX

　　这是一篇采用机器学习的点云配准的论文，简单来说就是通过将点云用一个个球体分成很多个小快，对每一块投影成深度图，然后采用深度神经网络对深度图进行特征压缩，最后压缩成一个5×2的矩阵做为一个特征，也就是我们所说的描述子（descriptor）。通过这些描述子的位置关系可以进行粗配准，最后文章也还是采用了ICP进行精配准。原文链接：3D Point Cloud Registration for Localization Using a Deep Neural Network Auto-Encoder。具体的内容我单独写了一篇博客：阅读笔记

4点法（4-Points Congruent Sets，4PCS）

　　4点法由D Aiger，NJ Mitra，D Cohen-Or$^{[1]}$于2008年提出的一种快速地，鲁棒地的3D点云粗配准方法，原文链接见 4-points Congruent Sets for Robust Surface Registration
占个位，这部分内容可见我的阅读笔记 4-Points Congruent Sets for Robust Pairwise Surface Registration——4PCS阅读笔记

Super 4PCS(Super 4-Points Congruent Sets)

　　Super 4PCS是由Nicolas Mellado，Dror Aiger，Niloy J. Mitra$^{[2]}$等人与2014年提出的一种对4PCS的加速方案。原文连接见Super4PCS: Fast Global Pointcloud Registration via Smart Indexing。
　　网上很少能找到关于这篇文章的原理，我个人的理解是首先它通过记录点集$P$中$ab$,$cd$两条直线相交的角度$\theta$，在点集$Q$中寻找匹配对时，额外计算两条直线相交的角度是否在$\theta$的一定范围$\xi$里，从而排除一些无效的匹配对来加速配准。
　　那么如何快速的找到角度在一定范围内的匹配对呢，Super4PCS采用的是类似与在球面上画圆的方法，如下图：
　　

　

球面上在圆$c$上的点，与点$e$连接产生的向量$\vec v$,与向量$\vec n$之间的夹角，均为$\theta$。若圆$c$具有一定的宽度，则圆上所有点与$e$组成的向量与向量$\vec n$的夹角分布在$[\theta - \xi,\theta + \xi]$。这样就可以快速找到交叉角度在一定范围内的两条直线。那么如何快速找到圆面上的这些点，Super4PCS则是通过光栅化，智能索引来完成，如下图：

这里原文中是通过二维来展示算法，不停的细分网格（cell）,将点云归一化塞入其中创建快速索引。这一部分不是很理解，欢迎大家来交流。

SK-4PCS（Semantic Keypoint 4-Points Congruent Sets）

在提取了上表面后，使用一系列高度间隔为$h_t$的平行面去分割点云，分割示意图如下图所示，其中绿色部分为其中一平行面：

对与分割面内的点，首先对其高度统一化，然后把他们看作2D图像，进行2D的特征提取和分割，如下图。其中红色点为线段顶点，绿色点为两条线的交点。

　　在提取了每个横截面中的特征点后，我们需要对它们进行区分和连接，根据他们自身的位置，以及潜在位置赋予不通的标签。最终建立$target$和$source$中语义点的意义对应关系。连接后的语义点如下图，其中蓝色线条连接特征点，红色点代表线段的顶点，绿色点代表线段的相交点，紫色三角形则是我们所定义的语义点，是连接线段与参考面的交点，存储着整条线段的语义信息（特征点的个数，每个特征点的标签）。

在搜寻到所有的SK-Point后，将这些SK-Points做为输入进行配对对的搜寻，不仅要满足原始的配准要求，同时其语义也要相同才能标记为匹配。

G-4PCS（Generalized 4-points congruent sets）

　　G-4PCS是由M Mohamad ， D Rappaport ， M Greenspan 提出的对4PCS的一种拓展，使4点法不在局限于共面的4点对。原文连接见：Super Generalized 4PCS for 3D Registration。
此方法不在限制4点须在一个平面内，下图显示了非共面的4点对，$S=\{p,q,i,j \}$，我们认为$pq$，$ij$空间相交，在$pq$上相交于$m$，在$ij$上相交于$n$，$mn$是$pq$到$ij$的最短距离。

则此方法不仅需要满足类似4点法的比例关系：

$$r_1=\dfrac{||q-m||}{||q-p||}$$
$$r_2=\dfrac{||i-n||}{||i-j||}$$
还需要满足下面这个附加条件：
$$d_3=||m-n||$$
满足以上三个条件才可认为是匹配对。

由于在4PCS中，强制选取共面的4点，所以由于点云对称导致的配准错的问题并不明显，但是在3D中，这种对称则会导致较为严重的错。如下图，假如一个基础对$\Gamma =\{i,j,p,q\}$以及3D交叉点$\{n,m\}$。

　　我们可以找到很多满足上式的匹配对，例如：$\Gamma_1=\{i,j,p_1,q_1\}$，$\Gamma_2=\{i,j,p_2,q_2\}$，$\Gamma_1=\{i,j,p_3,q_3\}$，分别相交于${n,m_1}$，${n,m_2}$，${n,m_3}$。因此，有必要在寻找匹配对时删去这些模糊的匹配对。在SK-4PCS中，作者对这种问题进行了分类，将其分为垂直类型和平面类型的两种模糊。
　　对于垂直类型的模糊，例如$\Gamma$和$\Gamma_1$，我们可以计算向量$\overrightarrow {nm}$和向量$\overrightarrow {nm_1}$,并和参考点云的向量做比较从而区分他们。
　　对于平面类型的模糊，如下图，我们定义一个通过向量$\overrightarrow {ij}$来定义一个参考方向，并计算方位角$\alpha_2$，$\alpha_3$从而进行区分。

精配准方案

　　精配准中，最经典最常用的方法就是迭代最近点法（Iterative Closest Point，ICP），而后的大多数方法都是ICP的变种。下文会简单介绍ICP算法的原理以及我在查阅论文中遇到的一些引用数较多的文章中所提到的算法。如果大家有什么经典或者新颖的算法也可以留言大家共同讨论交流，学习进步。

---

04/08/2018

DO（Discriminative Optimization）

　　与传统的构建损失函数，求解损失函数不同，　这篇文章提出了DO（Discriminative Optimization）的方法，通过学习更新序列来解决一系列计算机视觉上的问题。原文见2017年CVPR：Discriminative Optimization: Theory and Applications to Point Cloud Registration$^{[11]}$。
　　

1. 更新法则　

　　DO采用如下的更新法则：
　　

$$X_{k+1} = X_k - D_{k+1}h(X_k) \tag 2$$
　　其中， $h:\Bbb R^p \rightarrow \Bbb R^f$为特征提取函数，矩阵 $D_{k+1} \in \Bbb R^{p \times f}$将特征 $h(X_k)$转换为更新向量，矩阵 $D_{k+1}$由训练数据得来。

2. 训练方法

　　给定一个训练集${(X_0^{(i)},X_*^{(i)},h^{(i)})}_{i=1}^N$，其中$X_0^{(i)} \in \Bbb R^p$是第$i$个问题的实例，$X_*^{(i)} \in \Bbb R^p$为真实值（groundt ruth）,而$h^{(i)}：\Bbb R^p \rightarrow \Bbb R^f$为第$i$个实例的特征，DO的目标就是学习更新序列$\{D_k\}_k$，使得$X_0^{i}$更新为$X_*^{i}$，学习规则采用如下最小二乘回归函数：
　　

$$D_{k+1} = \arg \min_{\tilde D} \dfrac{1}{N} \sum ||X_*^{(i)}-X_k^{(i)} + \tilde D h^{(i)}(X_k^{(i)})||^2 \tag 2$$
　　在学习了 $D_{k+1}以后$，可以用（1）来更新 $X_k^{(i)}$，然后继续学习下一层，不断重复这个过程，直到满足终止条件。当 $X_k^{(i)} \approx X_*^{(i)}$，式（3）会强制 $\tilde D h^{(i)}(X_k^{(i)})$接近于0，也就是在 $X_*^{(i)}$附近引入驻点。在实际中，为了防止过拟合，加入了ridge regression:
　　 $$\min_{\tilde D} \dfrac{1}{N} \sum_{i=1}^{N} ||X_*^{(i)}-X_k^{(i)} + \tilde D h^{(i)}(X_k^{(i)})||^2 + \dfrac{\lambda}{2}||\tilde D||_F^2 \tag 3$$

3. 配准应用

　　设$M \in \Bbb R^{3 \times N_M}$为模型点云矩阵，$S \in \Bbb R^{3 \times N_S}$为场景模型矩阵。我们的目标是找到$S \rightarrow M$的最佳变化矩阵。
　　由于大部分感兴趣的点云都是由物体表面组成的，文中提出了一种基于统计场景中的点在模型点的前方还是后方的特征函数用于矩阵学习，如下图：
　　我们定义$\vec n_a \in \Bbb R^3$为模型点$m_a$由其领域计算得来的法向量。$\Gamma(y;x)$使用参数$x$对$y$做刚性变换。$m_a$的前方点定义为：$S_a^+=\{s_b:\vec n_a^{\top}(\Gamma(s_b;x)-m_a) \gt 0\}$，$S_a^-$则包含其他的场景点，对于$h:\Bbb R^6 \times \Bbb R^{3} \rightarrow \Bbb R^{2N_M}$定义如下：
　　

$$[h(x;S)]_a=\dfrac{1}{z} \sum_{s_b \in S_a^+} exp\bigl(- \dfrac{1}{\sigma^2}||\Gamma(s_b;x)-m_a ||^2\bigr) \tag 4$$
　　 $$[h(x;S)]_{a+N_M}=\dfrac{1}{z} \sum_{s_b \in S_a^-} exp\bigl(- \dfrac{1}{\sigma^2}||\Gamma(s_b;x)-m_a ||^2 \bigr) \tag 5$$
　　为了加速算法，输入数据会被归一化到 $[-1,1]$，训练数据来自对模型的随机采样，并进行随机变换做为ground truth进行训练。
　　
　　

结合法

　　结合法是将点云的颜色信息与几何信息相结合经行配准，原文见2017年ICCV：Colored Point Cloud Registration Revisited$^{[10]}$。文中回顾了RGB-D深度图像基于颜色的配准方案和基于几何位置的配准的方案，并将两者的相结合，并推广至无序点云。

1. RGB-D

　　一副RGB-D图像由颜色信息$I$和深度信息$D$组成，这里我们采用灰度信息来简化问题。给定一对RGB-D图像$(I_i,D_i)$，$(I_j,D_j)$以及一个粗略的初始矩阵$T^0$，我们的目标是找到一个使两幅图像紧密配准的最佳变换矩阵。
　　基于颜色信息的目标函数$E_I$为：
　　

$$E_I(T)=\sum {\bigl (I_i(x^{'}) - I_j(x)} \bigr)^2$$

　　其中，$x = (u,v)^T$是RGB-D图像$(I_j,D_j)$中的一个像素，而$x^{'} = (u^{'},v^{'})^T$是点$x$在图像$(I_i,D_i)$中的对应点。这种对应关系是通过将深度像素$(x,D_j(x))$在$(I_j,D_j)$的空间中转换为3D点，并将其变换$T$后投影到图像$(I_i,D_i)$中建立的，具体如下：
　　

$$x^{'} = g_{uv} \Bigl(s \bigl(h(x,D_j(x)),T \bigr)\Bigr)$$
　　其中， $h$是将深度像素转换为3D点，$s$为刚性变换， $g$为$h$的逆函数，将3D点转换为深度像素。
　　同样的，我们根据深度信息也可以如此定义目标函数 $E_D$，由两幅点云是在不同的相机空间中测量得到的，直接比较深度值会导致一个错误的结果， 所以这里我们采用弯曲深度。
　　 $$E_D()T = \sum_x \bigl(D_i(x^{'}) - g_d(s(h(x,D_j(x)),T)) \bigr)^2$$
　　如此我们可以通过下式 $E$，将两者进行结合，其中$\sigma \in [0,1]$为一常数，用于平衡两者间的关系。
　　 $$E(T) = (1 - \sigma)E_I(T) + \sigma E_D(T)$$

2. Color Point Cloud

　　接着，文章将上述方法拓展至普通点云。$P$为一有色点云，$C(p)$为颜色检索函数，我们需要将颜色建设函数转变为一个连续函数$C_p(u)$以便使用其梯度。
　　因此，文中为每一个点$p \in P$引入了虚拟正交相机这一概念，其沿着$\vec n_p$，观察点$p$，虚拟相机的观察面为点$p$的切平面，设$\vec u$是从点$p$开始的位于切平面上的一个向量，$\vec u \cdot \vec n_p =0$，则函数$C_p(u)$可一阶近似为：
　　

$$$C_p(u) \approx C(p) + d_p \top u$$
　　其中 $d_p$是函数 $(C_p(u))$的梯度，由对其临近点进行最小二乘拟合得来。我们定义 $f(s)$为投影3D点到切平面的函数：
　　 $$f(s) = s - \vec n_p(s-p)^{\top}\vec n_p$$
　　同样地我们假设虚拟相机有着深度信息因此定义连续深度函数 $G_p(u)$，他的原点的梯度为0， $0_p$为相机原点，则起一阶近似为：
　　 $$G_p(u) \approx (o_p - P)^\top \vec n_p$$
　　这样，对于两个点云 $P,Q$我们可以定义联合最优函数 $E(T)$:
　　 $$E(T) = (1-\sigma)E_C(T) + \sigma E_G(t)$$
　　令 $p,q$为每次迭代后 $P$和$T^kQ$的对应点集 $K=\{(p,q)\}$。 $q^{'}$为点 $q$在点$p$的切平面上的投影
　　 $$q^{'}=f(s(q,T)) 9$$
　　则 $E_C，E_G$分别如下定义：
　　 $$E_C(T)=\sum_{(p,q) \in K} (C_p(q^{'})-C(q))$$
　　 $$\begin{align} 　　E_G(T) & = \sum_{(p,q) \ in K } (G_p(q^{'})-(o_p - s()q,T))^\top \vec n_p)^2 \\ 　　& = \sum_{(p,q) \ in K }((s(q,T)-P)^\top \vec n_p)^2 　　\end{align}$$
　　如何求解，可以参考原文。同时为了避免陷入局部最优，文中采用不同大小的体素滤波器对原始点云进行滤波产生了不同精度的点云金字塔，通过不断提高输入的点云密度使得配准精度由粗略转为精细。

---

04/01/2018

ICP（Iterative Closest Point）

　　ICP算法是由PJ Besl ， ND Mckay $^{[6]}$在1992年提出的一种高层次的基于自由形态曲面的配准方法。原文连接见：A method for registration of 3-D shapes。简单来说就是不停的循环，在两个点云间找到最近的对应点，由这些对应点计算出一个使按照此旋转平移矩阵变换后对应点距离最小的RT矩阵，并进行变换，重复以上步骤，直到某些值达到设定的阈值则停止迭代。对于点集$P,Q$，算法步骤如下：

• 计算点集$Q_i$中的若干点在点集$P$中的最近点
• 求上述对应最近点对平均距离最小的旋转变化矩阵$RT_i$
• 对点集$Q_i$使用上述求的$RT_i$进行变换得到点集$Q_{i+1}$
• 若点集$P$与点集$Q_{i+1}$的某些目标函数小于一定阈值（例如最近点的平均距离），则可以停止迭代

模型对应法

　　好吧，这个名字是我起的。文章并没有给这个方法起一个名字。这篇文章是我在查阅资料时遇到的一篇100+引用的文章，原文是由T Rabbani ， S Dijkman ， FVD Heuvel ， G Vosselman 发表在ISPRS上的$^{[7]}$。原文连接见：An integrated approach for modelling and global registration of point clouds。这篇文章有趣之处在于提出了一种对点云进行建模与配准同时进行的方法。原文中有大量的公式推导，这里不会进行赘述。文章中针对工业环境存在大量的简单几何体例如平面，圆柱，圆环等实际提出两种方法，一种间接法，一种直接法。其中间接法是将物体拟合和场景匹配分开来做，而直接法则是同时决定物体的形状和位置参数以及配准参数。
　　　

1. 间接法

• 在不通的场景中为每个点赋予一个标记，将其标记在一个具体的物体上，文中采用Hoover, A., Jean-Baptiste, G. et al.在1996年提出的方案$^{[8]}$，An experimental comparison of range image segmentation algorithms。
• 在每个场景中，对每一个物体进行基于最小均方差的参数拟合
• 将物体参数做为输入，使用最小均方差法去计算最后的变换矩阵的参数，这一步将物体参数的均方差做为场景的变换参数的目标函数使其最小

数学公式可表示如下：

$$\min_{\{R\},\{t\}} \sum^{C}_{i=1} \sum^{M^i}_{j=1} \Delta^{2}_{u_{i_j}}$$
其中：
$$\Delta_{u_{i_j}}=\Psi_{R^a_i t^a_i}(u^a_{i_j}) -\Psi_{R^b_i t^b_i}(u^b_{i_j})$$

　　在等式中，$C$是指存在的对应关系的个数，每一个对应关系都包含有两个物体$a，b$。第$i$个对应关系的模型由$M_i$个拟合参数。$\Psi_{R^a_i t^a_i}$表示对场景使用变换对$R^a_i t^a_i$进行变换。物体$a$由它的参数$u^a_{i_j}$建模而成。我们的目标是使得存在对应关系的物体对的参数的均方差$\Delta^{2}_{u_{i_j}}$最小。因此，我们可以调整参数$\{R\}$，$\{t\}$。那么如何求解这个最值问题，具体可以参考原文的推导。

2. 直接法

• 配准参数预估，结果仅仅是对配准参数的预估
• 综合调整，配准参数和模型参数同时被估算

数学公式可表示如下：

$$\min_{\{R\},\{t\}} \sum^{O}_{i=1} \sum^{P^i}_{j=1} \Omega^2_i(R_i p_j + t_i)$$

其中，$O$是物体的总数，$P^i$是被用于第$i$个物体配准的点云 中所有未配准的点云中的点的个数。函数$\Omega_i$计算一个给定点到第$i$个物体表面的正交距离（orthogonal distance）。我们的目标是通过调整$\{R\}，\{t\}$使$\Omega^2_i$最小 。其中第二步来自Bundle Adjustmengt方法的拓展$^{[9]}$。

参考文献：

[1] Homepage B T. 4-points Congruent Sets for Robust Surface Registration[J].

[2] Mellado N, Aiger D, Mitra N J. Super 4PCS Fast Global Pointcloud Registration via Smart Indexing[J]. Computer Graphics Forum, 2015, 33(5):205-215.

[3] Ge X. Automatic markerless registration of point clouds with semantic-keypoint-based 4-points congruent sets[J]. Isprs Journal of Photogrammetry & Remote Sensing, 2017, 130:344-357.

[4] Theiler P W, Wegner J D, Schindler K. Keypoint-based 4-Points Congruent Sets – Automated marker-less registration of laser scans[J]. Isprs Journal of Photogrammetry & Remote Sensing, 2014, 96(11):149-163.

[5] Mohamad M, Ahmed M T, Rappaport D, et al. Super Generalized 4PCS for 3D Registration[C]// International Conference on 3d Vision. IEEE Computer Society, 2015:598-606.

[6] Besl P J, Mckay N D. A Method for Registration of 3-D Shapes[M]. IEEE Computer Society, 1992.

[7] Rabbani T, Dijkman S, Heuvel F V D, et al. An integrated approach for modelling and global registration of point clouds[J]. Isprs Journal of Photogrammetry & Remote Sensing, 2007, 61(6):355-370.

[8] Hoover A, Jean-Baptiste G, Jiang X, et al. An experimental comparison of range image segmentation algorithms[J]. IEEE Trnasactions on Pattern Analysis & Machine Intelligence, 1996, 18(7):673-689.

[9] Atkinson K B. Close Range Photogrammetry and Machine Vision[J]. Empire Survey Review, 2001, 34(266):276-276.

[10] Park J, Zhou Q Y, Koltun V. Colored Point Cloud Registration Revisited[C]// IEEE International Conference on Computer Vision. IEEE Computer Society, 2017:143-152.

[11] Vongkulbhisal J, Torre F D L, Costeira J P. Discriminative Optimization: Theory and Applications to Point Cloud Registration[C]// IEEE Conference on Computer Vision and Pattern Recognition. IEEE, 2017:3975-3983.