rigid.m

求刚体变换的刚体矩阵的开源程序

function [R,t]=rigid(P,Q)
  u=size(P);N=u(2);
  l=[0;0;0];m=zeros(u);
  k=l;n=m;t=k;
  for i=1:N
      l=l+P(:,i);
  end
      l=l/N;
  for j=1:N
      m(:,j)=P(:,j)-l;
  end
  for i=1:N
      k=k+Q(:,i);
  end
      k=k/N;
  for j=1:N
      n(:,j)=Q(:,j)-k;
  end
  R=round(m,n);
  t=k-R*l;
    function r=round(A,B)
      u=size(A);N=u(2);
      C=zeros(3,3);
      for j=1:3
          for i=1:N
          C(:,j)=C(:,j)+B(j,i)*A(:,i);
          end
      end
       [x,y,z]=svd(C);
       r=z*x';