0、前言
在说 Point-to-Plane ICP 之前，先介绍一下 ICP：
经典ICP
形如求相机的旋转矩阵一样，ICP的目的很简单，就是求解两堆点云之间的变换关系。具体的做法是，既然不知道 R和t (针对刚体运动)，那我们就假设为未知量呗，然后通过某些方法求解。
假设有两堆点云，分别记为两个集合 X=x1,x2,…,xm 和 Y=y1,y2,…,ym（m并不总是等于n）。然后呢，我们不失一般性的，假设两个点云之间的变换为 R（旋转变换）和t（平移变换），这个问题描述成最小化均方误差：
经典的ICP方法对上面的优化问题的处理思路如下：
（1）初始化R 和 t
确定初始的R 和 t 的方法很多， 初始值的选取很重要，如果初始值选的不好很容易收敛到一个局部最优解 。ICP发展了这么多年了，当然有很多的方法来估计初始的R和t了，像PCL给的SampleConsensusInitalAlignment函数和TransformationEstimationSVD函数都可以得到较好的初始估计。
（2）迭代
得到初始的估计后，接下来的步骤就顺理成章了：对于X中的每一个点用当前的R 和 t在Y中找最近的点（比如用欧式距离），得到最近的点后，对所有的点都这么做一次，然后我们就得到了所有的匹配对。，用每一对的坐标列一个方程，就得到一系列的方程。然后就求解最优的R和t最小化上面的误差。如此循环往复---最小二乘的思想，最终得到瞒住我们设定的最小距离