%-------------------------------------------------------------
% Jana Kosecka
% UC Berkeley
% April 1999
% 8 point algrithm for fundamental matrix estimation
% p - fisrt image coordinates
% q - second image coordinates
% F - fundamental matrix
% F = [f1 f4 f7
% f2 f5 f8
% f3 f6 f9]
% epipolar constraint of the form q'*F*p = 0;
% optional scaling of the image coordinates for better
% conditioning

% Algorith, from Chapter 6, "An introduction to 3-D Vision"
% by Y. Ma, S. Soatto, J. Kosecka, S. Sastry (MASKS)
%
% Code distributed free for non-commercial use
% Copyright (c) MASKS, 2003
% ---------------------------------------------------------------------

function F = dfundamental(p, q)

scaling = 1;
NPOINTS = size(p,2);

if scaling
Kinv(:,:,1) = chol(inv(p*p'/size(p,2)));
p = Kinv(:,:,1)*p;
Kinv(:,:,2) = chol(inv(q*q'/size(q,2)));
q = Kinv(:,:,2)*q;
else
Kinv(:,:,1)=eye(3,3);
Kinv(:,:,2)=eye(3,3);
end
A = zeros(NPOINTS, 9);

if NPOINTS < 9
error('Too few mesurements')
return;
end

for i = 1:NPOINTS
A(i,:) = kron(p(:,i),q(:,i))';
end
r = rank(A);

if r < 8
error('Measurement matrix rank defficient');
end;

[U,S,V] = svd(A);

% pick the eigenvector corresponding to the smallest eigenvalue
f = V(:,9);
est = reshape(f,3,3);
f = Kinv(:,:,2)'*est*Kinv(:,:,1);

[U,S,V] = svd(f);
S(3,3) = 0;
F = U*S*V';

F = F/norm(F,'fro');