三维点云配准

点云数据

点云数据通常表示为N行，至少3列的矩阵，其中N表示点的数量，每一行代表一个点。通常3列分别是点在空间中（x，y，z）的坐标。如果点云数据有除空间中坐标外的附加信息，如来自LIDAR传感器的点云数据，那么它可能具有每个点的附加值，例如“反射率”，其是在该位置中障碍物反射多少激光光束的量度。 在这种情况下，点云数据可能是N×4阵列。

三维点云配准

点云的配准过程，就是求两个点云之间的一个旋转平移矩阵，源点云通过旋转平移矩阵后，变换成和目标点云矩阵相同的矩阵。

可以表示为以下方程：

其中，pt，ps分别是目标点云和源点云中的一对对应点。
R，T就是我们要求的旋转平移矩阵。
但是，我们并不知道两个点云中，点与点之间的对应关系，这也是配准的核心问题。

三维点云配准分为粗配准与精配准两步。
粗配准就是在完全不清楚两个点云的相对位置关系的情况下，找到一个这两个点云近似的旋转平移矩阵（不一定很精确，但是已经大概是对的了）。
精配准就是在已知一个旋转平移矩阵的初值的情况下（这个初值大概已经是正确的了），进一步计算得到更加精确的旋转平移矩阵。

配准的评价标准：比较通用的是LCP(Largetst Common Pointset)（最大公共点集）。给定两个点集P，Q，找到一个变换T，使得变换后的T（P）与Q的重叠度最大。在变换后的P内任意一点，如果在容差范围内有另外一个Q的点，则认为该点是重合点。重合点占所有点数量的比例就是重叠度。

（一）粗配准

（1）暴力配准

粗配准中，最简单粗暴的一个方法就是暴力配准。找到一个刚体变换需要3对对应点，即在点集P中选取3个点，如果我们能找到它们在点集Q中的对应点，即能根据它们的坐标求出旋转平移矩阵。（具体怎么求：pass）

暴力配准步骤：

• 随机在点集P中选取3个点，随机在点集Q中选取3个点。
• 计算旋转平移矩阵T
• 计算 ||T（P）- Q||<$\delta$的点的个数$k_i$（点集P经过旋转平移矩阵后，点集中与点集Q的点的距离小于给定的阈值$\delta$的点的个数）
• 遍历所有的可能性，选取最高的$k_i$作为最后的结果

暴力配准的缺点：时间复杂度高。

假设点集P的大小为n，点集Q的大小为m，分别随机选取3个点组成关联对，则在选取3个点时就分别有$A_n^3$和$A_m^3$种可能，点集P中选取的任意一种可能，均需遍历点集Q中的所有可能。因此，完成整个算法共需遍历$A_n^3\times A_m^3=n(n-1)(n-2)m(m-1)(m-2)$种可能。因此，暴力配准的时间复杂度为$O(n^3{m}^3)$。（当n，m足够大时，-1,-2等都失去了意义）

（2）4PCS（4-Points Congruent Sets）（4点全等集）

4PCS的核心原理在于，在刚性变换中，同一平面上的4个点，在经过刚性变换后，其某些比率是稳定不变的。

如图，{a，b，c，d}四点共面，其中两条直线的交点为e。在刚性变换中，r1，r2是稳定不变的。

4PCS步骤：

• 在点集P中选取共面四点对作为base B={a，b，c，d}

• 计算比率$r_1$，$r_2$

• 点集Q（n个点）中，每两个点组成一对（$C_n^2$），对于每一对点$q_1$，$q_2$，计算他们的中间点
  $$e_1=q_1+r_1(q_2-q_1)$$$$e_2=q_1+r_2(q_2-q_1)$$
  
  如图，每一对点都可以计算出两个中间点$e_1,e_2$

• 若点集Q中，任意两对点，一对由$r_1$计算得到的中间点$e_1$和另一对由$r_2$计算得到的中间点$e_2$的距离在允许范围内，那么可以认为这两对点可能是base B={a，b，c，d}的仿射对应点。
  
  如图，此时可认为点集{$q_1$，$q_2$，$q_3$，$q_4$}是base B={a，b，c，d}的仿射对应点

• 可能存在k个这样的点集，这k个点集再作如下处理：
  （1）根据每个点集与base B，计算出旋转平移矩阵T
  （2）计算 ||T（P）- Q||<$\delta$的点的个数$k_i$（点集P经过旋转平移矩阵后，点集T（P）与点集Q，距离小于给定的阈值$\delta$的点的个数）
  （3）遍历所有k个点集，选取最高的$k_i$作为最后的结果

因此，4PCS算法的时间复杂度包括两部分，点集Q（n个点）中任意选取两个点组成一对，计算$e_1,e_2$，需作$A_n^2=n(n-1)次$，在找出的$k$个可能是base B的仿射对应点的四点集中，遍历检查，获得最好的旋转平移矩阵T。$O(n^2+k)$

（二）精配准
精配准的模式基本已固定为使用ICP算法（Iterative Closest Point）（最近点迭代法）及其各种变种。
ICP算法的步骤：

• （1）ICP算法的核心是最小化一个目标函数：$$f(R,T)=\frac{1}{N_p}\sum _{i=1}^{N_P}|p_t^i-R\cdot p_s^i-T{|}^2$$
  $p_t^i，p_s^i$是一对对应点，总共有$N_p$对对应点，这个目标函数实际上是找到旋转平移矩阵R，T，使得所有对应点之间的欧式距离的平方和最小。

• （2）寻找对应点。起初并不知道有哪些对应点，因此，在有粗配准获得的旋转平移矩阵的初值下，用此旋转平移矩阵对源点云进行变换，得到一个变换后的点云，将这个变换后的点云与目标点云进行比较，只要两个点云中存在距离小于一定阈值（ICP算法的一个参数）的点，就认为这两个点是对应点。（最邻近点）

• （3）R，T优化。有了对应点后，根据目标函数优化R，T。

• （4）迭代。优化得到了新的R，T，导致变换后的点云的一些点的位置发生了变化，一些最近邻点对也相应地发生了变化，回到步骤（2）中重新寻找对应点。迭代（2）（3）步骤，直到满足迭代终止条件，如R，T的变化量小于一定值，或目标函数的变化小于一定值，或最近邻点对不再改变等（ICP算法的另一个参数）

• （5）迭代终止后，最终获得精配准的R，T

ICP算法的参数：
（1）距离阈值。两个点云中距离小于距离阈值的点，才算作对应点。
（2）迭代的终止条件。

至此，完成点云数据的配准。

参考文献：
[1] Aiger D, Mitra N J, Cohen-Or D. 4-points congruent sets for robust pairwise surface registration[J]. Acm Transactions on Graphics, 2008, 27(3):85.
[2]https://blog.csdn.net/u011600592/article/details/70258097