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; [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)); [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 figure(2); plot(1:iter,delta_Intermediate(1:iter,1)'); xlabel('迭代次数');ylabel('delta'); figure(3); plot(1:iter,e_Intermediate(1:iter)'); xlabel('迭代次数');ylabel('loss'); %% %计算最终的R与T temp_R=eye(3); temp_T=zeros(3,1); 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_final,1,size(data1,1)); 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;