vb_affine_trans_for_fit

 Affine transformation matrix to fit X to Y 
  A = vb_affine_trans_for_fit(X,Y)
  A = vb_affine_trans_for_fit(X,Y,flag)
 --- Input
 X : 3D cordinate of N points [N x 3]
 Y : 3D cordinate of N points [N x 3]
 --- Optional input
 flag = 0 (affine transform) , 1 (Rigid transform)
 if flag=1, return rigid transformation (rotation + translation)
    
 --- Output
 A : affine transformation matrix from X to Y [4 x 3]
 Xout = [X , ones(N,1)] * A
      : affine transform of X such that Xout and Y becomes close each other
 
 Masa-aki Sato 2008-2-27

 Copyright (C) 2011, ATR All Rights Reserved.
 License : New BSD License(see VBMEG_LICENSE.txt)

0001 function    A = vb_affine_trans_for_fit(X,Y,flag)
0002 % Affine transformation matrix to fit X to Y
0003 %  A = vb_affine_trans_for_fit(X,Y)
0004 %  A = vb_affine_trans_for_fit(X,Y,flag)
0005 % --- Input
0006 % X : 3D cordinate of N points [N x 3]
0007 % Y : 3D cordinate of N points [N x 3]
0008 % --- Optional input
0009 % flag = 0 (affine transform) , 1 (Rigid transform)
0010 % if flag=1, return rigid transformation (rotation + translation)
0011 %
0012 % --- Output
0013 % A : affine transformation matrix from X to Y [4 x 3]
0014 % Xout = [X , ones(N,1)] * A
0015 %      : affine transform of X such that Xout and Y becomes close each other
0016 %
0017 % Masa-aki Sato 2008-2-27
0018 %
0019 % Copyright (C) 2011, ATR All Rights Reserved.
0020 % License : New BSD License(see VBMEG_LICENSE.txt)
0021 
0022 
0023 N = size(X,1);
0024 if size(Y,1) ~= N, error('Data size error'); end;
0025 if size(X,2) ~= 3 | size(Y,2) ~= 3 , error('Vector size error'); end;
0026 
0027 X  = [X , ones(N,1)];
0028 XX = X' * X;
0029 YX = Y' * X;
0030 
0031 % affine transformation matrix
0032 A  = YX / XX;
0033 A  = A';
0034 
0035 if ~exist('flag','var') | flag==0, 
0036     return
0037 end
0038 % Rotation matrix
0039 R = A(1:3,1:3);
0040 R = vb_repmultiply(R , 1./sqrt(sum(R.^2,2)))
0041 
0042 R(3,:) = [R(1,2)*R(2,3)-R(1,3)*R(2,2) , ...
0043           R(1,3)*R(2,1)-R(1,1)*R(2,3) , ...
0044           R(1,1)*R(2,2)-R(1,2)*R(2,1)];
0045 
0046 R(3,:) = R(3,:)/sqrt(sum(R(3,:).^2));
0047 
0048 R(1,:) = [R(2,2)*R(3,3)-R(2,3)*R(3,2) , ...
0049           R(2,3)*R(3,1)-R(2,1)*R(3,3) , ...
0050           R(2,1)*R(3,2)-R(2,2)*R(3,1)];
0051 
0052 A(1:3,1:3) = R;
0053 
0054 %Xout = X * A';