%%
%尋找的變換關系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