% camera calibration by assuming projection is a conjugate of a rotation
% and then estimation of this rotation, followed by choleski factorization
%
% Use: k = ______(P)

function k = test(P)
[M,N]=size(P);
if N~= 8
  error('       : P must have a width of 8')
end%if

A = [];  % this is the matrix of knowns in Ac=0
for m = 1:M
  p=[P(m,:) 1];   % lengthen with 1 at end for notational simplicity in A
  Pmat = [p(1) p(2) p(3); p(4) p(5) p(6); p(7) p(8) 1];
  Qmat = inv(Pmat.');   % Q = P^(-T)
  Qmat = Qmat/Qmat(3,3);
  q = [Qmat(1,1) Qmat(1,2) Qmat(1,3) Qmat(2,1) Qmat(2,2) Qmat(2,3) ...
       Qmat(3,1) Qmat(3,2) 1];

  A = [A;...  % concatenate 6 rows for each m
       p(1)-q(1)  p(2)-q(4)  p(3)-q(7)  0          0          0; ...
       0-q(2)  p(1)-q(5)     0-q(8)  p(2)       p(3)       0; ...
       0-q(3)  0-q(6)     p(1)-q(9)  0          p(2)       p(3); ...
       p(4)    p(5)-q(1)  p(6)       0-q(4)     0-q(7)     0; ...
       0       p(4)-q(2)  0          p(5)-q(5)  p(6)-q(8)  0; ...
       0       0-q(3)     p(4)       0-q(6)     p(5)-q(9)  p(6); ...
       p(7)    p(8)       p(9)-q(1)  0          0-q(4)     0-q(7); ...
%       0       ?p(4)       0-q(2)     ?p(5)       ?p(6)-q(5)  0-q(8); ...
       0       p(7)       0-q(2)     p(8)       p(9)-q(5)  0-q(8); ...
       0       0          p(7)-q(3)  0          p(8)-q(6)  p(9)-q(9); ...
      ];
end%for

AA = A.'*A; % the eigenvector corresponding to the least eigenvalue of
            % this 6 by 6 matrix.
[V,D]=eig(AA);   % since AA is hermitian (it's even real and symmetric)
                 % then D should be real
                 % also since AA is positive semidefinite, D should be non neg.
d=diag(D);
[trash,ind]=min(d);
c=conj(V(:,ind)); % this is the LstSqr solution of Ac=0
                  % inner product of conj this column with x s.b. 1
                  % for normalized c

C=[c(1) c(2) c(3); c(2) c(4) c(5); c(3) c(5) c(6)];

if (eig(C)) < 0
  disp('c, when arranged as C, is has a negative eigenvalue')
  disp('therefore, assuming it is negative definite, and using -C')
  K=chol(-C);
else%if
  K=chol(C);
end%if

K=K/K(3,3);
k=[K(1,1) K(1,2) K(1,3) K(2,1) K(2,2) K(2,3) K(3,1) K(3,2)];