本文算法原理参考于：

核心思想：使用四个近似共面点来描述两个三维点云之间的刚性变换关系，因此由称”四点匹配算法“。即不停迭代计算源点云Q的四点集Qa，与目标点云P中与Qa重叠度最高的四点集Pb，二者之间的最优变换矩阵T，得到粗配准的变换矩阵。

如下图：

1.点云四点集的确定

（1）选择三个共面点

1）在源点云中随机选择三个点，要求这三点所构成的三角形面积尽量的大（三点确定一个平面，向量叉积可以计算面积），但是三点间的距离不能超过一定的阈值上限，该上限由两片点云的给定重叠率 f 确定。

2）因为三点之间距离越大，配准的鲁棒性越高；但距离过大，三点均在两点云的重叠区域之外了，配准效果又不好。因此需要在满足上限的基础之上，尽可能保证大的三级形面积。

3）若没有给定点云重叠率f，则可以进行f=1.0,0.75,0.5...重叠率递减测试，选择最优变换。

（2）选择第四个共面点

1）确定三点后，遍历源点云中所有的点，对每一个点进行计算验证选择最优的第四点。

2）第四点需要与其他三点组成的平面尽可能的共面（即不强制要求四点共面，但第四点到其他三点平面的距离尽可能小），并且第四点与其他三点的距离也要满足距离阈值范围（不能太近不能太远）。

2.4PCS计算

计算其四点构成的两线段交点的变换不变比，根据不变比在目标点云中遍历搜索对应的满足该约束所有四点集，得到的四点集，即近似全等四点集。

具体流程：

（1）选取源点云四点集

1）在源点云P中，使用上述的四点集的选择方法随机选择一个四点集B={b1,b2,b3,b4}，其中(b1,b2)确定线段1，(b3,b4)确定线段2。

2）计算不变量d1=|b1-b2|，d2=|b3-b4|（约束1），不变比r1=|b1-e| / |b1-b2|，r2=|b3-e| / |b3-b4|（约束2）。注意因为四点不一定共面，两条线段也不一定相交，所以可以使用连接两个线段的最近点的中心点作为“交点”。

（2）遍历目标点云点对，筛选得到目标点云中的点对集合。

1）在目标点云Q中，遍历所有的点对，筛选满足约束1（允许有一定的误差）的点对集合R1，R2。其表示为：

计算R1 ={ (pi, pj) | pi, pj ∈ Q) }，使|| pi - pj ||∈ [ d1- δ,d1+δ ].

计算R2 ={ (pi, pj) | pi, pj ∈ Q) }，使|| pi - pj ||∈ [ d2- δ,d2+δ ].

（3）构建映射

（4）求解近似全等四点集

（5）得到本次迭代最优变换矩阵T

2）使用该变换矩阵对源点云P进行变换得到P'，统计P'与Q中的最大公共点集（LCP），记max(LCP)的变换矩阵为本次迭代的最优变换矩阵T并保留。

（6）不断迭代这个过程，记录最优的变换。迭代结束后得到的变换矩阵即为最优变换矩阵。

（1）设置源和目标之间的近似重叠度

（2）设置常数因子delta，用于对内部计算的参数进行加权

（3）设置验证配准效果时要使用的采样点数量