function [R,t] = hornQuar(A,B)
% Registers two sets of 3DoF data
% Assumes A and B are d,n sets of data
% where d is the dimension of the system 
% typically d = 2,3
% and n is the number of points
% typically n>3
%
% Mili Shah
% July 2014

[d,n]=size(A);

%Mean Center Data
Ac = mean(A')';
Bc = mean(B')';
A = A-repmat(Ac,1,n);
B = B-repmat(Bc,1,n);

%Calculate Optimal Rotation
M = A*B';
N = [M(1,1) + M(2,2) + M(3,3) M(2,3)-M(3,2) M(3,1)-M(1,3) M(1,2)-M(2,1);...
    M(2,3)-M(3,2) M(1,1)-M(2,2)-M(3,3) M(1,2)+M(2,1) M(3,1)+M(1,3);...
    M(3,1)-M(1,3) M(1,2)+M(2,1) -M(1,1)+M(2,2)-M(3,3) M(2,3)+M(3,2);...
    M(1,2)-M(2,1) M(3,1)+M(1,3) M(2,3)+M(3,2) -M(1,1)-M(2,2)+M(3,3)];
[u,v]=eig(N);
[m,ind]=max(diag(v));
r = u(:,ind);
r = [r(2:4);r(1)];
R = (r(4)^2-r(1:3)'*r(1:3))*eye(3) + 2*r(1:3)*r(1:3)'+2*r(4)*[0 -r(3) r(2);r(3) 0 -r(1);-r(2) r(1) 0];
%Calculate Optimal Translation
t = Bc - R*Ac;
end