% hartley constraint
% finds the calibration matrix from a collection of "p" vectors...

function K=dummy(p)

[M,N]=size(p);
if N~=8
  error('input parameter list must be an M by 8 matrix')
end%if
if M<=3
  error('input parameter list should be quite long to get a good estimate')
end%if

X=[];   % initialize the ``X'' of Hartley, page 8
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;

  P=[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];
  Q=inv(P');
  Q=Q/Q(3,3);
  q=[Q(1,1) Q(1,2) Q(1,3) Q(2,1) Q(2,2) Q(2,3) Q(3,1) Q(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;
  Xm=[q11-p11    q21-p12    q31-p13    0          0          0      ;...
      q12        q22-p11    q32           -p12       -p13    0      ;...
      q13        q23        q33-p11    0             -p12       -p13;...
...
         -p21    q11-p22       -p23    q21        q31        0      ;...
      0          q12-p21    0          q22-p22    q32-p23    0      ;...
      0          q13           -p21    q23        q33-p22       -p23;...
...
         -p31       -p32    q11-p33    0          q21        q31    ;...
      0             -p31    q12            -p32   q22-p33    q32    ;...
      0          0          q13-p31    0          q23-p32    q33-p33;...
     ];
  X=[X;Xm];

end%for

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

C=[answer(1) answer(2) answer(3);...
   answer(2) answer(4) answer(5);...
   answer(3) answer(5) answer(6);...
  ];
%%%K=chol(C); wrong because C=KK' not K'K
eig(C)
K = inv(chol(inv(C)));
K=K/K(3,3);