% Algorithm 11.8 
% estimate focal length using absolute quadric constrainst
% the remaining paratemters of calibration matrix are assumed known
% Described in Chapter 11, "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 Omega = quadric_linear_f(P,varargin);
% P - projection matrices
% Omega - absolute quadric

function Omega = quadric_linear_f(P,varargin);

 if ~isempty(varargin) ar = varargin{1}; else
    ar = 1;
 end;
  ar ;
  PC = P;
  FRAMES = size(PC,3);
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % case 1 only focal length is unknown
  % set up the constraints on absolute quadric
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  for j=2:FRAMES
   i = 4*(j-2) + 1;
   D(i,:) = 2*row_quadric_f(PC(1,:,j), PC(2,:,j));
   D(i+1,:) = 2*row_quadric_f(PC(1,:,j), PC(3,:,j));
   D(i+2,:) = 2*row_quadric_f(PC(2,:,j), PC(3,:,j));
   D(i+3,:) = (ar^2)*row_quadric_f(PC(1,:,j), PC(1,:,j))-row_quadric_f(PC(2,:,j), PC(2,:,j));
  end;
  A = D(:,1:5);
  b = -D(:,6);
  t = A\b;
  ttt = t/norm(t);

if 0
[u2, s2, v2] = svd(D);
eigenvaluesofD = s2(5:6,5:6)
%  randDD = rank(D);
vv = v2(:,6)/v2(6,6);
t = vv(1:5);
tttt = t/norm(t);
end

ff = [t(1), t(2), t(3), t(4), t(5)];
norm(t(2:4))^2;
     
Omega = [t(1)  0    0   t(2);
            0   t(1)  0   t(3);
            0    0    1   t(4);
	     t(2) t(3) t(4) t(5)]

[u1,s1,v1] = svd(Omega);
s1(4,4) = 0;
Omega = u1*s1*v1'