for iter=1:iteration
%寻找控制点的对应点
for i=1:controldatanum
    temp_data1=repmat(controldata1(i,:),m,1);
    diff=sqrt(sum((temp_data1-data2).^2,2));
    [minvalue,index(i,1)]=min(diff);
    controldata2(i,:)=data2(index(i,1),:);
end
%%
%对于确定的关系，求解RT
centroid1=mean(controldata1);
centroid2=mean(controldata2);
demeancontroldata1=controldata1-repmat(centroid1,controldatanum,1);
demeancontroldata2=controldata2-repmat(centroid2,controldatanum,1);
H=demeancontroldata1'*demeancontroldata2;
[U,S,V]=svd(H);
R=V*U';
T=(centroid2-centroid1)';
R_Intermediate(:,:,iter)=R;
T_Intermediate(:,:,iter)=T;
%%
%利用求解得到的RT计算变换之后的点
controldata1=R*controldata1'+repmat(T,1,controldatanum);
controldata1=controldata1';%新的控制点
E=norm(controldata1-controldata2,2);
e_Intermediate(iter,1)=E/controldatanum
delta=abs(E-last_E)/controldatanum%中间迭代的误差
delta_Intermediate(iter,1)=delta;
if(delta<0.001)
    break;
end
last_E=E;
end

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

配准的结果如下图：

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

不足之处

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

全部代码

%%
%寻找的变换关系data2=Rdata1+T

%%
%加载数据选取控制点
data1=load('3.asc');
data2=load('4.asc');
figure(1);
plot3(data1(:,1),data1(:,2),data1(:,3),'r.');
hold on;
plot3(data2(:,1),data2(:,2),data2(:,3),'b.');
title('原始数据');
axis tight equal;
hold off;


[m,n]=size(data2);
controldata1=load('controldata.asc');%选取控制点
[controldatanum,~]=size(controldata1);
controldata2=zeros(controldatanum,3);
%%
%初始化
R=[1,0,0;0,1,0;0,0,1];
T=[0,0,0];
last_E=0;
iteration=20;
R_Intermediate=zeros(3,3,iteration);
T_Intermediate=zeros(3,1,iteration);
delta_Intermediate=zeros(iteration,1);
index=zeros(controldatanum,1);
e_Intermediate=zeros(iteration,1);
%%
%迭代
for iter=1:iteration
%寻找控制点的对应点
for i=1:controldatanum
    temp_data1=repmat(controldata1(i,:),m,1);
    diff=sqrt(sum((temp_data1-data2).^2,2));