Matlab实现经典的ICP点云拼接配准算法

function result = init(X,Y) pointx=size(X,2); pointy=size(Y,2); %------分别求解X Y的每一行的均值------% xc = mean(X,2);% xc:3*1的矩阵 yc = mean(Y,2); %mean(A,2)求各行的均值 %----分别求解两个数据集合的协方差矩阵----% x1 = X - repmat(xc,1,pointx); %repmat(A,m,n)将矩阵A以m行n列复制摆好拼起来 Mx =x1 * x1'; %x1':转置矩阵 y1 = Y - repmat(yc,1,pointy); My = y1 * y1'; %------求解Mx My的特征值和特征向量------% [Vx,Dx] = eig(Mx,'nobalance'); %Vx特征向量,Dx特征值 [Vy,Dy] = eig(My,'nobalance'); %R存在问题 %判断特征向量矩阵方向？？ [~,index]=max(sum(x1.*x1)); xm=x1(:,index); xm(3,1)=-abs(xm(3,1)); p3 = Vx(:,3); if dot(xm,p3)<0 p3=-p3; end p2 = Vx(:,2); if dot(xm,p2)<0 p2=-p2; end p1=cross(p3,p2); % Vx=[p1,p2,p3]; % xx=Vx*x1; % if std(xx(3,xx(3,:)>0),1)<std(xx(3,xx(3,:)<0),1) % p3=-p3; % end % if std(xx(2,xx(2,:)>0),1)<std(xx(2,xx(2,:)<0),1) % p2=-p2; % end % p1=cross(p3,p2); [~,index]=max(sum(y1.*y1)); ym=y1(:,index); ym(3,1)=-abs(ym(3,1)); q3 = Vy(:,3); if dot(ym,q3)<0 q3=-q3; end q2 = Vy(:,2); if dot(ym,q2)<0 q2=-q2; end q1=cross(q3,q2); % q3 = Vy(:,3); % f = 0.8; %f是阀值 % if dot(p1,q1) < f %dot（x,y）向量点乘 % p1 = -p1; % end % if dot(p2,q2)<f % p2 = -p2; % end % % p3 = cross(p1,p2);%返回向量叉积 R = [q1,q2,q3]/[p1,p2,p3];%R=(q1,q2,q3)(p1,p2,p3)-1：逆矩阵 %T重新求 % T = (yc - xc); % xc2 = mean(s*R*X,2); xc2 = R*xc; T = (yc - xc2); result = cell({R;T});