rigid_transform.m

WSN中DOA算法的mds定位

function [alpha,T,r]=rigid_transform(M,N)%N表示要进行旋转的坐标系统
p=size(M,1);%点的个数
if p==0;
    disp(M);
    disp(N);
end
    A_1=0;
    A_2=0;
    B_1=0;
    B_2=0;
 for i=1:p
    for j=1:p
        A_1=A_1+(N(i,1)*M(j,2)-N(i,2)*M(j,1));
        B_1=B_1+(-N(i,1)*M(j,1)-N(i,2)*M(j,2));
    end
    A_2=A_2+(-N(i,1)*M(i,2)+N(i,2)*M(i,1));
    B_2=B_2+(N(i,1)*M(i,1)+N(i,2)*M(i,2));
 end
 A=(A_1/p)+A_2;
 B=(B_1/p)+B_2;
 if B==0
     disp(M);
     disp(N);
 end
 alpha_1=atan(-A/B);
 alpha_2=atan(-A/B)+pi;
 %t1_1=0;t1_2=0;
 %t2_1=0;t2_2=0;
 %for i=1:p
   %  t1_1=t1_1+cos(alpha_1)*N(i,1)-sin(alpha_1)*N(i,2)-M(i,1);
    % t1_2=t1_2+cos(alpha_2)*N(i,1)-sin(alpha_2)*N(i,2)-M(i,1);
     %t2_1=t2_1+cos(alpha_1)*N(i,2)+sin(alpha_1)*N(i,1)-M(i,2);
     %t2_2=t2_2+cos(alpha_2)*N(i,2)+sin(alpha_2)*N(i,1)-M(i,2);
 %end
 %t1_1=-t1_1/p;
 %t1_2=-t1_2/p;
 %t2_1=-t2_1/p;
 %t2_2=-t2_2/p;
 t1_1=-mean(cos(alpha_1).*N(:,1)-sin(alpha_1).*N(:,2)-M(:,1));
 t1_2=-mean(cos(alpha_2).*N(:,1)-sin(alpha_2).*N(:,2)-M(:,1));
 t2_1=-mean(cos(alpha_1).*N(:,2)+sin(alpha_1).*N(:,1)-M(:,2));
 t2_2=-mean(cos(alpha_2).*N(:,2)+sin(alpha_2).*N(:,1)-M(:,2));
% U_1=zeros(1,p);U_2=zeros(1,p);
 %V_1=zeros(1,p);V_2=zeros(1,p);
 %for i=1:p
    % U_1(i)=cos(alpha_1)*N(i,1)-sin(alpha_1)*N(i,2)-M(i,1);
     %U_2(i)=cos(alpha_2)*N(i,1)-sin(alpha_2)*N(i,2)-M(i,1);
     %V_1(i)=cos(alpha_1)*N(i,2)+sin(alpha_1)*N(i,1)-M(i,2);
     %V_2(i)=cos(alpha_2)*N(i,2)+sin(alpha_2)*N(i,1)-M(i,2);
 %end
%  for a=-0.05:0.001:0.05;
 %for b= -0.05:0.001:0.05;
  %   temp1=sum((U_1+(t1_1+a).*ones(1,p)).^2+(V_1+(t2_1+b).*ones(1,p))).^2;
    % temp2=sum((U_2+(t1_2+a).*ones(1,p)).^2+(V_2+(t2_2+b).*ones(1,p))).^2;
    %E1(i)=sum(U_1(i)+)
    %E2(i)=
 %end
  %end
  %[m1,n1]=sort(E1,'ascend');
  %[m2,n2]=sort(E2,'ascend');
 %options=optimset();
%T_1=fminsearch(@opt,[t1_1;t2_1],options,U_1,V_1);%计算出alpha_1的情况
%T_2=fminsearch(@opt,[t1_2;t2_2],options,U_2,V_2);%计算出alpha_2的情况
%t1_1=T_1(1);
%t2_1=T_1(2);
%t1_2=T_2(1);
%t2_2=T_2(2);
 E_1=0;
 E_2=0;
 for i=1:p
     E_1=E_1+(cos(alpha_1)*N(i,1)-sin(alpha_1)*N(i,2)-M(i,1)+t1_1).^2+(cos(alpha_1)*N(i,2)+sin(alpha_1)*N(i,1)-M(i,2)+t2_1).^2;
     E_2=E_2+(cos(alpha_2)*N(i,1)-sin(alpha_2)*N(i,2)-M(i,1)+t1_2).^2+(cos(alpha_2)*N(i,2)+sin(alpha_2)*N(i,1)-M(i,2)+t2_2).^2;
 end
 if E_1<E_2
     t1_0=t1_1;
     t2_0=t2_1;
     alpha_0=alpha_1;
 else
      t1_0=t1_2;
     t2_0=t2_2;
     alpha_0=alpha_2;
 end
 N(:,2)=-N(:,2);
    A_1=0;
    A_2=0;
    B_1=0;
    B_2=0;
 for i=1:p
    for j=1:p
        A_1=A_1+(N(i,1)*M(j,2)-N(i,2)*M(j,1));
        B_1=B_1+(-N(i,1)*M(j,1)-N(i,2)*M(j,2));
    end
    A_2=A_2+(-N(i,1)*M(i,2)+N(i,2)*M(i,1));
    B_2=B_2+(N(i,1)*M(i,1)+N(i,2)*M(i,2));
 end
 A=(A_1/p)+A_2;
 B=(B_1/p)+B_2;
 if B==0
     disp(M);
     disp(N);
 end
 alpha_1=atan(-A/B);
 alpha_2=atan(-A/B)+pi;
% t1_1=0;t1_2=0;
% t2_1=0;t2_2=0;
% for i=1:p
   %  t1_1=t1_1+cos(alpha_1)*N(i,1)-sin(alpha_1)*N(i,2)-M(i,1);
    % t1_2=t1_2+cos(alpha_2)*N(i,1)-sin(alpha_2)*N(i,2)-M(i,1);
     %t2_1=t2_1+cos(alpha_1)*N(i,2)+sin(alpha_1)*N(i,1)-M(i,2);
     %t2_2=t2_2+cos(alpha_2)*N(i,2)+sin(alpha_2)*N(i,1)-M(i,2);
 %end
 %t1_1=-t1_1/p;
 %t1_2=-t1_2/p;
 %t2_1=-t2_1/p;
 %t2_2=-t2_2/p;  
  t1_1=-mean(cos(alpha_1).*N(:,1)-sin(alpha_1).*N(:,2)-M(:,1));
 t1_2=-mean(cos(alpha_2).*N(:,1)-sin(alpha_2).*N(:,2)-M(:,1));
 t2_1=-mean(cos(alpha_1).*N(:,2)+sin(alpha_1).*N(:,1)-M(:,2));
 t2_2=-mean(cos(alpha_2).*N(:,2)+sin(alpha_2).*N(:,1)-M(:,2));
%  U_1=zeros(1,p);U_2=zeros(1,p);
 %V_1=zeros(1,p);V_2=zeros(1,p);
 %for i=1:p
    % U_1(i)=cos(alpha_1)*N(i,1)-sin(alpha_1)*N(i,2)-M(i,1);
    %U_2(i)=cos(alpha_2)*N(i,1)-sin(alpha_2)*N(i,2)-M(i,1);
     %V_1(i)=cos(alpha_1)*N(i,2)+sin(alpha_1)*N(i,1)-M(i,2);
     %V_2(i)=cos(alpha_2)*N(i,2)+sin(alpha_2)*N(i,1)-M(i,2);
 %end
 %ans=sum((U_1+t1_1).^2+(V_1+t2_1).^2);
 %options=optimset();
%T_1=fminsearch(@opt,[t1_1;t2_1],options,U_1,V_1);%计算出alpha_1的情况
%T_2=fminsearch(@opt,[t1_2;t2_2],options,U_2,V_2);%计算出alpha_2的情况
%t1_1=T_1(1);
%t2_1=T_1(2);
%t1_2=T_2(1);
%t2_2=T_2(2);
 E_1=0;
 E_2=0;
 for i=1:p
     E_1=E_1+(cos(alpha_1)*N(i,1)-sin(alpha_1)*N(i,2)-M(i,1)+t1_1).^2+(cos(alpha_1)*N(i,2)+sin(alpha_1)*N(i,1)-M(i,2)+t2_1).^2;
     E_2=E_2+(cos(alpha_2)*N(i,1)-sin(alpha_2)*N(i,2)-M(i,1)+t1_2).^2+(cos(alpha_2)*N(i,2)+sin(alpha_2)*N(i,1)-M(i,2)+t2_2).^2;
 end
 if E_1<E_2
     t1_r=t1_1;
     t2_r=t2_1;
     alpha_r=alpha_1;
 else
      t1_r=t1_2;
     t2_r=t2_2;
     alpha_r=alpha_2;
 end
 N(:,2)=-N(:,2);
 P=0;Q=0;
 for i=1:size(N,1)
     P=P+norm([cos(alpha_0),-sin(alpha_0);sin(alpha_0),cos(alpha_0)]*(N(i,:))'+[t1_0;t2_0]-(M(i,:))').^2;
     Q=Q+norm([cos(alpha_r),-sin(alpha_r);sin(alpha_r),cos(alpha_r)]*[1,0;0,-1]*(N(i,:))'+[t1_r;t2_r]-(M(i,:))').^2;
 end
     if P<=Q
     alpha=alpha_0;
     T=[t1_0,t2_0];
     r=0;
     else
        alpha=alpha_r;%进行了非法修改
     T=[t1_r,t2_r];%进行了非法修改
     r=1;
     end