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

[d,n]=size(A);

C1 = zeros(4,4);
C2 = n*eye(4);
C3 = zeros(4,4);
for i = 1:n
    C1 = C1 + -2*Q([1/2*B(:,i);0])'*W([1/2*A(:,i);0]);
    C3 = C3 + 2*(W([1/2*A(:,i);0])-Q([1/2*B(:,i);0]));
end
A = 1/2*(C3'*inv(C2+C2')*C3-C1-C1');
[u,v]=eig(A);
[m,ind]=max(diag(v));
r = u(:,ind);
s = -inv(C2+C2')*C3*r;
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];
t = W(r)'*s*2;
t = t(1:3);
end

function Q = Q(r)
K = [0 -r(3) r(2);r(3) 0 -r(1);-r(2) r(1) 0];
Q = [r(4)*eye(3)+K r(1:3);-r(1:3)' r(4)];
end
function W = W(r)
K = [0 -r(3) r(2);r(3) 0 -r(1);-r(2) r(1) 0];
W = [r(4)*eye(3)-K r(1:3);-r(1:3)' r(4)];
end