function [R,t] = horn(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 = B*A';
R = M*(M'*M)^(-1/2);
if det(R)<0, disp('Warning: R is a reflection'); end

%Calculate Optimal Translation
t = Bc - R*Ac;
end