ICP算法本质上是基于最小二乘法的最优配准方法。该算法重复进行选择对应关系点对，计算最优刚体变换这一过程，直到满足正确配准的收敛精度要求。ICP是一个广泛使用的配准算法，主要目的就是找到旋转和平移参数，将两个不同坐标系下的点云，以其中一个点云坐标系为全局坐标系，另一个点云经过旋转和平移后两组点云重合部分完全重叠。ICP算法最早由Chen and Medioni，and Besl and McKay提出。

算法的输入：参考点云和目标点云，停止迭代的标准。 算法的输出：旋转和平移矩阵，即转换矩阵。

对于目标点云中的每个点，匹配参考点云（或选定集合）中的最近点。 求得使上述对应点对计算均方根（root mean square，RMS）最小的刚体变换，求得平移参数和旋转参数。 使用获得的转换矩阵来转换目标点云。 迭代（重新关联点），直到满足终止迭代的条件（迭代次数或误差小于阈值）。这里的误差最小，可以是相邻两次均方根差的绝对值小于某一限差。

法收敛于局部最小误差。 噪声或异常数据可能导致算法无法收敛或错误。 在进行ICP算法第一步要确定一个迭代初值，选取的初值将对最后配准结果产生重要的影响，如果初值选择不合适，算法可能就会限入局部最优，使得迭代不能收敛到正确的配准结果。

旋转矩阵是欧式空间中执行旋转变换的矩阵，ICP算法在做点云配准时，变换矩阵包含了平移和旋转，因此，这里有必要研究一下坐标系的平移和旋转过程。三维变换与二维变换相似，采用齐次坐标技术来描述空间的各点坐标及其变换，描述空间三维变换的变换矩阵是4×4的形式。由此，一系列变换可以用单个矩阵来表示。

以坐标系的三个坐标轴X,Y,ZX,Y,Z分别作为旋转轴，则点实际上只在垂直坐标轴的平面上作二维旋转。此时用二维旋转公式就可以直接推出三维旋转变换矩阵。规定在右手坐标系中，物体旋转的正方向是右手螺旋方向，即从该轴正半轴向原点看是逆时针方向。

绕三轴的基本旋转矩阵，选装角度分别位θ,α,βθ,α,β:

假设用Qi,i=1,2,3,⋯Qi,i=1,2,3,⋯表示第一个点集，Pi,i=1,2,3,⋯Pi,i=1,2,3,⋯表示第二个点集。两个点集的对齐配准转换为使下列目标函数最小。

其中，旋转矩阵R和平移矩阵T，就是找到的待配准点云数据与参考点云数据之间的旋转参数和平移参数,使得两点集数据之间满足某种度量准则下的最优匹配。

ICP配准过程： 1. 计算P中的每一个点在Q点集中的对应最近点 2. 求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数 3. 对PP使用上一步求得的平移和旋转参数，得到新的变换点集P′P′ 4. 如果新的变换点集与参考点集满足上面目标函数要求，即两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集P′P′作为新的PP继续迭代，直到达到目标函数的要求。

提示：这里的目标函数的要求，也可以是迭代次数。