step1：选择控制点pi→∈P、设置T的初始值T0=T0

step2：重复执行以下步骤直至满足收敛条件

step2-1：对各控制点，pi→在Q中求其最近点qj→并将其作为pi→的假想对应点

step2-3：重新计算控制点pi→在經过Tk变换之后的点，并将其重新赋值给pi→

经过初始配准之后的数据集：

在初始时通过手动选取3000个点作为配准点；平移通过质心の间的距离计算，旋转通过svd分解进行计算具体实现方法为，假设两组去质心的数据点为qi与q′i通过计算

由于控制点手工选取的较好，所以算法收敛的很快基本上经过三次迭代即收敛，算法的loss函数定义为控制点与其match之间的平均差距即为Ek/M，得到如下图像

配准结果好坏的衡量标准为观察点云是否融合在一起仔细观察上述结果是彼此融合在一起。

用matlab数值法解方程实現起来速度比较慢尤其是在寻找控制点的match时，需要对另外一组数据进行遍历匹配一种比较快速的方法是用C++并且通过K-d tree进行匹配点搜索，這样耗时应该比较少

%利用求解得到的RT计算变换之后的点