clear all

%%First define the function crossm which we need. 
crossm=inline('[0 -x(3) x(2);x(3) 0 -x(1);-x(2) x(1) 0]','x'); 

%syms t1 t2 r0 r1 r2
%
%rot = [r0;r1;r2];
%T   = [1;t1;t2];

motion_mult = 1e-8;
U = [randn(3,5); ones(1,5)];
P0 = [eye(3) zeros(3,1) ];
dR = crossm( motion_mult*rand(3,1) );
dT = motion_mult*randn(3,1);
P1 = P0 + [ dR dT];

u0 = P0*U;
u0 = u0./(ones(3,1)*u0(end,:));

u1 = P1*U;
u1 = u1./(ones(3,1)*u1(end,:));

u = (u0+u1)/2;
up = u1-u0;


SOLS =  fivepoint_infinitestimal(u,up);

reproj=[]; 
for i=1:size(SOLS,2)
  Test = SOLS(4:6,i); 
  Rest = crossm( SOLS(1:3,i)); 
  for j=1:5
    Z = zeros(3,1); 
    M = [eye(3) Z u(:,j) Z; Rest Test up(:,j) u(:,j)];
    reproj = [reproj det(M)]; 
  end  
end
reprojmaxerr = max(abs(reproj))

err = SOLS(4:6,:) - (dT/dT(1))*ones(1,10);
[Terrmin , I ] = min( sqrt( sum( err.^2) )  );
%SOLS(:,I) 
%dT/dT(1)
Terrmin
Rerrmin = norm( crossm( SOLS(1:3,I)  ) - dR ) /norm( dR )