ICP_matlab

基于ICP的迭代配准算法，用matlab编写

%%
%寻找的变换关系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;


[mn]=size（data2）;
controldata1=load（‘controldata.asc‘）;%选取控制点
[controldatanum~]=size（controldata1）;
controldata2=zeros（controldatanum3）;
%%
%初始化
R=[100;010;001];
T=[000];
last_E=0;
iteration=20;
R_Intermediate=zeros（33iteration）;
T_Intermediate=zeros（31iteration）;
delta_Intermediate=zeros（iteration1）;
index=zeros（controldatanum1）;
e_Intermediate=zeros（iteration1）;
%%
%迭代
for iter=1:iteration
%寻找控制点的对应点
for i=1:controldatanum
    temp_data1=repmat（controldata1（i:）m1）;
    diff=sqrt（sum（（temp_data1-data2）.^22））;
    [minvalueindex（i1）]=min（diff）;
    controldata2（i:）=data2（index（i1）:）;
end
%%
%对于确定的关系，求解RT
centroid1=mean（controldata1）;
centroid2=mean（controldata2）;
demeancontroldata1=controldata1-repmat（centroid1controldatanum1）;
demeancontroldata2=controldata2-repmat（centroid2controldatanum1）;
H=demeancontroldata1‘*demeancontroldata2;
[USV]=svd（H）;
R=V*U‘;
T=（centroid2-centroid1）‘;
R_Intermediate（::iter）=R;
T_Intermediate（::iter）=T;
%%
%利用求解得到的RT计算变换之后的点
controldata1=R*controldata1‘+repmat（T1controldatanum）;
controldata1=controldata1‘;%新的控制点
E=norm（controldata1-controldata22）;
e_Intermediate（iter1）=E/controldatanum
delta=abs（E-last_E）/controldatanum%中间迭代的误差
delta_Intermediate（iter1）=delta;
if（delta<0.001）
    break;
end
last_E=E;
end

figure（2）;
plot（1:iterdelta_Intermediate（1:iter1）‘）;
xlabel（‘迭代次数‘）;ylabel（‘delta‘）;
figure（3）;
plot（1:itere_Intermediate（1:iter）‘）;
xlabel（‘迭代次数‘）;ylabel（‘loss‘）;
%%
%计算最终的R与T
temp_R=eye（3）;
temp_T=zeros（31）;
for i=1:iter
   temp_R=R_Intermediate（::i）*temp_R; 
   temp_T=R_Intermediate（::i）*temp_T+T_Intermediate（::i）;
end
R_final=temp_R;
T_final=temp_T;

data1_transformed=R_final*data1‘+repmat（T_final1size（data11））;
data1_transformed=data1_transformed‘;
figure（4）;
plot3（data1_transformed（:1）data1_transformed（:2）data1_transformed（:3）‘r.‘）
hold on;
plot3（data2（:1）data2（:2）data2（:3）‘b.‘）
title（‘ICP results‘）
axis equal tight;
hold off;
 save data3.asc -ascii data1_transformed;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件    5470577  2017-01-03 11:47  ICPmatlab\3.asc

     文件    5411799  2017-01-01 18:50  ICPmatlab\4.asc

     文件     106109  2017-01-03 16:04  ICPmatlab\controldata.asc

     文件       2428  2017-01-03 17:00  ICPmatlab\main.m

    ..AD...         0  2017-01-07 16:15  ICPmatlab

     文件    7793400  2017-01-03 16:06  ICPmatlab\data3.asc

----------- ---------  ---------- -----  ----

             18784313                    6