function [q,qi] = Fau2d(calpts)
%
% Fau2d: Least squares fit to determine perspective transform 3x3 as a plane
%        to plane transformation
% Method:
%          [Ui Vi Si]' = Q*[xi yi 1]'
%
%    =>    ui = Ui/Si = (xi*q11+yi*q12+q13)/(xi*q31+yi*q32+q33)
%          vi = Vi/Si = (xi*q21+yi*q22+q23)/(xi*q31+yi*q32+q33)
%
%   gives 2 equations for each calibration pair:
%
%                                         |q11|
%                                         |q12|
%                     ...                 |q13|      ...
%        |xi yi 1  0  0 0 -ui*xi -ui*yi|  |q21|     |ui|
%        | 0  0 0 xi yi 1 -vi*xi -vi*yi|  |q22|  =  |vi|
%                     ...                 |q23|      ...
%                                         |q31|
%                                         |q32|
% where we have taken q33=1, and thus need a minimum of 4 points.
%
% On input:
%     calpts: m by 4 matrix with corresponding world point - image point pairs
% On output:
%     q: forward transform (takes world points into image coordinates)
%    qi: inverse transform (takes image coordinates into world coordinates)
%        [Note that these points are not normalized and should be scaled
%         so that their z component is 1]
% Call:
%       [q,qi] = Fau2d(calpts)
%

[m,n] = size(calpts);
A = zeros(2*m,8);
b = zeros(2*m,1);
M = calpts(:,1:2);
u = calpts(:,3:4);
row = 0;
for i1 = 1:m
  row = row+1;
  b(row) = u(i1,1);
  A(row,1) = M(i1,1);
  A(row,2) = M(i1,2);
  A(row,3) = 1;
  A(row,7) = -u(i1,1)*M(i1,1);
  A(row,8) = -u(i1,1)*M(i1,2);
  row = row+1;
  b(row) = u(i1,2);
  A(row,4) = M(i1,1);
  A(row,5) = M(i1,2);
  A(row,6) = 1;
  A(row,7) = -u(i1,2)*M(i1,1);
  A(row,8) = -u(i1,2)*M(i1,2);
end;

q = A\b;
p = [q(1:3)'; q(4:6)'; q(7:8)' 1];
if rank(p)>2
  qi = inv(p);
else
  disp('No inverse');
end
