% version that only solves focal length

function c=aaa(p)

[M,N]=size(p);
if N~=8
  error('hartley2: parameter list must be an M by 8 matrix')
end%if
if M<=3
  error('hartley2: parameter list must be long enough to give good estimate')
end%if

Q=[];   % initialize
P=[];
for m=1:M
  p11=p(m,1); p12=p(m,2); p13=p(m,3);
  p21=p(m,4); p22=p(m,5); p23=p(m,6);
  p31=p(m,7); p32=p(m,8); p33=1;

  %%% q=pinverse(p(m,:));  wrong: should be conjtranspose inverse
  thisP=[p(m,1) p(m,2) p(m,3); p(m,4) p(m,5) p(m,6); p(m,7) p(m,8) 1];
  thisQ=inv(thisP');
  thisQ=thisQ/thisQ(3,3);
  q=[thisQ(1,1) thisQ(1,2) thisQ(1,3) thisQ(2,1) thisQ(2,2) thisQ(2,3)...
                                             thisQ(3,1) thisQ(3,2)];
  q11=q(1); q12=q(2); q13=q(3);
  q21=q(4); q22=q(5); q23=q(6);
  q31=q(7); q32=q(8); q33=1;

  Qm=[q11 0;...
      q12 0;...
      q13 0;...
...
      q21 0;...
      q22 0;...
      q23 0;...
...
      q31 0;...
      q32 0;...
      q33 0 ...
     ];
  Q=[Q;Qm];

  Pm=[0   0;...
      0   0;...
      0   p13;...
...
      0   0;...
      0   0;...
      0   p23;...
...
      0   0;...
      0   0;...
      0   p33...
     ];
  P=[P;Pm];

end%for

X=Q-P; % the ``X'' of Hartley, page 8

[v d]=eig(X'*X)
d=diag(d);
[trash location]=min(d);  % choose least eigenvalue
answer=v(:,location);

c=answer(1)/answer(2);