% solves for the 3x3 rotation matrix R and 3x1 translation
% vector T given 3*n  (n>2) matrices Y and X of corresponding
% points ideally satisfying Y = R*X + T
function [R,T] = solvepose3(X,Y)

  % solve for position
  Ymean = mean(Y')';
  Xmean = mean(X')';
  K = (Y - Ymean*ones(3,1)')*(X - Xmean*ones(3,1)')';
  [V,D,U] = svd(K);
  D = eye(3,3);
  D(3,3) = det(V*U');
  R = V*D*U';
  T = Ymean - R*Xmean;
