1.对点云的靶标进行读取和显示
%读取靶标矩阵
A = importdata('babiao.xlsx')
[X1,Y1,Z1] = size(A)
x = A(:,1);
y = A(:,2);
z = A(:,3);
plot3(x,y,z,'+')
grid on
2.这里先认为将靶标做空间变换,来验证ICP算法是否适用
%做空间变换
eul = [pi/4 pi/5 pi/3];
rotm1 = eul2rotm(eul,'ZYX');
T = [x,y,z];
T2 = T*rotm1;
x = T2(:,1);
y = T2(:,2);
z = T2(:,3);
plot3(x,y,z,'x',)
grid on
3.利用ICP匹配:
data1 = A;
data2 = T2;
control = A(1:6,:);
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= control;
[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=50;
R_Intermediate=zeros(3,3,iteration);
T_Intermediate=zeros(3,1,iteration);
delta_Intermediate=zeros(iteration,1);
index=zeros(controldatanum,1);
e_Intermediate=ze