20-3-26 下午3:01 G:\code\my_ICP.m 第 1 页，共 5 页 1 %% 2 %两个三维点集的最小二乘拟合 3 %%2020.3.1 4 tic 5 clear 6 close all; 7 clc 8 %% 9 %原始数据 10 Data_1=load('bun045.asc'); 11 Data_2=load('bun000.asc'); 12 %%加载控制点 13 [m,n]=size(Data_2); 14 [cgx,wmy]=size(Data_1); 15 xx=300;%选择控制点数 16 idx=randperm(cgx); 17 idx=idx(1:xx); 18 A=Data_1(idx,:); 19 %control_data_1=load('controldata.asc'); 20 control_data_1=A; 21 [control_data_num,~]=size(control_data_1);%data1控制点个数 22 control_data_2=zeros(control_data_num,3);%data2控制点矩阵 23 %% 24 %R\T\E初始化 25 %E旋转矩阵 26 %T平移矩阵 27 %E_begain2end每次迭代的精度 28 R=eye(3); 29 T=zeros(3,1); 30 begain_E=0; 31 iteration=50;%迭代次数 32 R_begain2end=zeros(3,3,iteration);%记录每次迭代R 33 T_begain2end=zeros(3,1,iteration); 34 E_begain2end=zeros(iteration,1); 35 delta_begain2end=zeros(iteration,1); 36 index=zeros(control_data_num,1); 37 38 %% 39 %迭代 40 %寻找控制点的对应点 

20-3-26 下午3:01 G:\code\my_ICP.m 第 2 页，共 5 页 41 for i=1:iteration 42 %% 43 %每个控制点计算一次距离 44 for j=1:control_data_num 45 %%根据控制点生成与参考点云Data2数据量一致的点云与Data2的数据进行计算 46 tem_Data_1=repmat(control_data_1(j,:),m,1);%控制点的第j行排列成m行1列 47 % tem_Data_1=str2num (tem_Data_1_); 48 distance=sqrt(sum((tem_Data_1-Data_2).^2,2));%计算每个点的距离 49 [min_distance,index(j,1)]=min(distance);%最近点 50 control_data_2(j,:)=Data_2(index(j,1),:); 51 end 52 %%求R\T 53 zhixin_1=mean(control_data_1);%质心 54 zhixin_2=mean(control_data_2); 55 %两组控制点去质心 56 sub_control_data_1=control_data_1-repmat(zhixin_1,control_data_num,1); 57 sub_control_data_2=control_data_2-repmat(zhixin_2,control_data_num,1); 58 %H=q*q' 59 H=sub_control_data_1'*sub_control_data_2; 60 [U,S,V]=svd(H); 61 R=V*U'; 62 T=(zhixin_2-zhixin_1)'; 63 R_begain2end(:,:,i)=R; 64 T_begain2end(:,:,i)=T; 65 %%用R和T做变换 66 control_data_1=R*control_data_1'+repmat(T,1,control_data_num);%编程 3*controldatanum矩阵 67 control_data_1=control_data_1';%变回来controldatanum*3 68 E=norm(control_data_1-control_data_2,2);%返回距离最大值 69 E_begain2end(i,1)=E/control_data_num; 70 delta=abs(E-begain_E)/control_data_num; 71 delta_begain2end(i,1)=delta; 72 if(delta<0.000000000000001) 73 break; 74 end 75 begain_E=E; 76 end 77 figure(2) 78 plot(1:i,delta_begain2end(1:i,1)','r'); 79 xlabel('迭代次数');ylabel('delta'); 

20-3-26 下午3:01 G:\code\my_ICP.m 第 3 页，共 5 页 80 figure(3) 81 plot(1:i,E_begain2end(1:i)','r'); 82 xlabel('迭代次数');ylabel('loss函数'); 83 %%计算最后的R\T 84 temp_R=eye(3); 85 temp_T=zeros(3,1); 86 for s=1:i 87 temp_R=R_begain2end(:,:,s)*temp_R; 88 temp_T=R_begain2end(:,:,s)*temp_T+T_begain2end(:,:,s); 89 end 90 R_end=temp_R; 91 T_end=temp_T; 92 data1=R_end*Data_1'+repmat(T_end,1,size(Data_1,1)); 93 data1=data1'; 94 %% 95 %模拟数据测试 96 R0=[1 0.001 0.001;0.001 1 0.001;0.001 0.001 1]; 97 T0=[0 0 0]; 98 data0=R0*Data_1'+repmat(T0',1,size(Data_1,1)); 99 data0=data0'; 100 [R11,T11]=icp1(Data_1,data0); 101 dR=R11-R0; 102 dT=T11-T0'; 103 %R111=inv(R11); 104 %save data0.asc -ascii; 105 %fprintf(data0.asc,formatSpec); 106 %%计算旋转角 107 if abs(R_end(3,1)) ~= 1 108 theta1 = -asin(R_end(3,1)); 109 theta2 = pi - theta1; 110 psi1 = atan2(R_end(3,2)/cos(theta1), R_end(3,3)/cos(theta1)); 111 psi2 = atan2(R_end(3,2)/cos(theta2), R_end(3,3)/cos(theta2)); 112 pfi1 = atan2(R_end(2,1)/cos(theta1), R_end(1,1)/cos(theta1)); 113 pfi2 = atan2(R_end(2,1)/cos(theta2), R_end(1,1)/cos(theta2)); 114 theta = theta1; 115 psi = psi1; 116 pfi = pfi1; 117 else 118 phi = 0; 119 delta = atan2(R_end(1,2), R_end(1,3)); 

20-3-26 下午3:01 G:\code\my_ICP.m 第 4 页，共 5 页 120 if R_end(3,1) == -1 121 theta = pi/2; 122 psi = phi + delta; 123 else 124 theta = -pi/2; 125 psi = -phi + delta; 126 end 127 end 128 eulerAngles = [psi*180/pi theta*180/pi pfi*180/pi]; 129 130 %% 131 %计算均方误差RMS 132 P = data1'; 133 Q = Data_2'; 134 k = knnsearch(Q',P'); 135 Z = Q(:,k); 136 c = P-Z; 137 e = mean(dot(c,c)); 138 e = sqrt(e); 139 disp(['RMS=',num2str(e)]); 140 %%显示原始数据 141 figure(1); 142 plot3(Data_1(:,1),Data_1(:,2),Data_1(:,3),'r.'); 143 hold on; 144 plot3(Data_2(:,1),Data_2(:,2),Data_2(:,3),'b.'); 145 title('原始数据'); 146 axis tight equal; 147 hold off; 148 %% 149 %配准结果 150 figure(4) 151 plot3(data1(:,1),data1(:,2),data1(:,3),'r.'); 152 hold on; 153 plot3(Data_2(:,1),Data_2(:,2),Data_2(:,3),'b.'); 154 title('ICP配准结果') 155 axis equal tight; 156 hold off; 157 toc 158 159 

20-3-26 下午3:01 G:\code\my_ICP.m 第 5 页，共 5 页