参考网址：https://wenku.baidu.com/view/08833045d15abe23482f4da4.html
经典ICP算法右如下7个步骤：
（1）根据带匹配的点集合P={P1， P2， P3，...，Pn}的点坐标，在模板数据点集合S={S1， S2，S3，...，}中搜索相应最近点的集Q={q1， q2，...，qn}。
ICP算法假设带匹配的两组扫描数据是完全重叠的，但是激光测距仪所测的数据是离散的，对应点之间并没有十分严格的匹配。因此，为了增加有效点的重叠部分，并且缩短计算时间，选取激光测距仪扫描一轮所测所测数据的正前方120度范围的数据点作为待匹配数据的点集P。
根据待匹配数据点集P中的数据点pi去搜索模板点集S中与该点最近的一点保存为qi。
图1 临近点搜索
两点之间的距离平方和为dis。
其中，qj遍历整个点集S，通过不断地比较，找到最近的点，根据该点序列号保存在点集Q中，记为qi。
（2）算两个点集P、Q的重心位置坐标，并进行点集中心化，生成新的点集。
计算两个点集合P、Q的重心位置坐标，即所有横坐标和取平均，所有纵坐标和取平均，得到P、Q的重心坐标d（x，y），m（x，y）
将点集P、Q中所有点坐标扣除各自集合的重心坐标，得到对应的新的点集D、M；
（3）由新的点集计算协方差矩阵，并求解由它组成的一个四元数矩阵的最大特征值及其最大特征向量。（协方差矩阵见本博客其他篇章）
定义一个矩阵A和T。。。
定义四元素矩阵B。。。（怎么插入公式呢？）
求解旋转矩阵，找到四元素矩阵对应的最大特征值和对应的特征向量
（4）由于最大特征向量等价于残差平方和最小时的旋转四元素，将四元素转换为旋转矩阵R；
（5）旋转矩阵R被确定后，由评议向量T仅仅是两个点集的重心差异，可以通过两个坐标系中的中心点和旋转矩阵确定。
（6）将带匹配数据点集P按照计算的旋转矩阵R和平移矩阵T变换后形成新的点集P，通过新的点集P与与模板的临近点集Q计算所有对应点距离平方和值除以数据点总数的值I作为迭代判断数值。
（7）当迭代判断数值I小于阈值时，ICP配准算法就停止跌打，否则重复（1）至（6）步。
这就是所有的过程了！
可以参考的一些文章https://www.cnblogs.com/wangguchangqing/p/8287585.html