function H=shahOne(A,B)
%Solves the problem YA = B
%using the formulation of
%
%Comparing Two Sets of Corresponding 
%Six Degree of Freedom Data
%M. Shah
%
%Mili Shah
%July 2014

%Centroid of transformations t and that

[m,n]=size(A); n = n/4;
t = 1/n*sum(A(1:3,4:4:4*n)')';
that = 1/n*sum(B(1:3,4:4:4*n)')';
A(1:3,4:4:4*n) = A(1:3,4:4:4*n)-repmat(t,1,n);
B(1:3,4:4:4*n) = B(1:3,4:4:4*n)-repmat(that,1,n);

%Calculates the best rotation
[u,s,v]=svd(A(1:3,:)*B(1:3,:)');
R = v*u';
if det(R)<0
    disp(sprintf('warning: reflection for H'))
    R = v*diag([1,1,-1])*u';
end

%Calculates the best transformation
t = that-R*t;

%Puts everything together to form
%the homogeneous transformation
H = [R,t;0 0 0 1];