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});
评论7
最新资源