function [p,T,alphau,alphav,u0,v0] = Faugeras(calpts)
%
% Faugeras: camera calibration after Faugeras (3D Computer Vision)
%           
% On input:
%     calpts: set of 3D world points and image points (m by 5 matrix)
% On output:
%     p: perspective transform matrix
%     T: transform matrix (3D transfrom from 3D frame to optical center)
% alphau: -f*ku   (f: focal length; ku: spacing in u direction)
% alphav: -f*kv   (f: focal length; kv: spacing in v direction)
%    u0: u coord of intersection of optical axis with retinal plane
%    v0: v coord of intersection of optical axis with retinal plane
% Call:
%     [p,T,alphau,alphav,u0,v0] = Faugeras(calpts)
%

X = zeros(10,10); Y = zeros(10,10);
[m,n] = size(calpts);
p = zeros(3,4);
A = zeros(2*m,12);
B = zeros(2*m,3);
C = zeros(2*m,9);

for ptset = 1:m
  row1 = (ptset-1)*2+1;
  row2 = row1+1;
  A(row1,1)  = calpts(ptset,1);
  A(row1,2)  = calpts(ptset,2);
  A(row1,3)  = calpts(ptset,3);
  A(row1,4)  = 1.0;
  A(row1,9)  = -calpts(ptset,4)*calpts(ptset,1);
  A(row1,10) = -calpts(ptset,4)*calpts(ptset,2);
  A(row1,11) = -calpts(ptset,4)*calpts(ptset,3);
  A(row1,12) = -calpts(ptset,4);
  
  A(row2,5)  = calpts(ptset,1);
  A(row2,6)  = calpts(ptset,2);
  A(row2,7)  = calpts(ptset,3);
  A(row2,8)  = 1.0;
  A(row2,9)  = -calpts(ptset,5)*calpts(ptset,1);
  A(row2,10) = -calpts(ptset,5)*calpts(ptset,2);
  A(row2,11) = -calpts(ptset,5)*calpts(ptset,3);
  A(row2,12) = -calpts(ptset,5);
end;

C = [A(:,1:8), A(:,12)];
if rank(C)>8
  D = A(:,9:11);
  Ctinv = inv(C'*C);
  E = D'*D - D'*C*Ctinv*C'*D;
  [Evecs,Evals] = eig(E);
  Emini = 1;  Eminv = Evals(1,1);
  for i1 = 2:3
    if Evals(i1,i1)<Eminv
      Emini = i1;
      Eminv = Evals(i1,i1);
    end;
  end;
  z = Evecs(:,Emini);
  y = -Ctinv*C'*D*z;
  p = [y(1:4)' ; y(5:8)' ; z' y(9)];

% Recover the rigid transformation from the world frame to the optical ctr

  q1 = y(1:3);
  q14 = y(4);
  q2 = y(5:7);
  q24 = y(8);
  q3 = z;
  q34 = y(9);
  
  u0 = dot(q1,q3);
  v0 = dot(q2,q3);
  tz = q34;
  R3 = q3';
  alphau = norm(cross(q1,q3));
  alphav = norm(cross(q2,q3));
  R1 = (q1'-u0*q3')/alphau;
  R2 = (q2'-v0*q3')/alphav;
  tx = (q14-u0*tz)/alphau;
  ty = (q24-v0*tz)/alphav;
  T = [R1 tx; R2 ty; R3 tz];
  
else
  disp('Calibration points inadequate');
end
