% est_qchirp2                                          Steve Mann, 1992-1994
%
% estimate the 10 Taylor series (incl. qchirp) parameters from a pair of images
%
% The model is:   u = p1 + p2 x + p3 y + p4 x^2 +  p5 xy
%                 v = q6 + q7 x + q8 y + q9 y^2 + q10 xy
%
% Use: p = est_qchirp2(E1,E2);
%  or: [p1,p2,p3,p4,p5,p6,p7,p8,p9,p10] = est_qchirp2(E1,E2);
%  or : est_qchirp2(E1,E2);      % nicely formatted screen output
%
% sign convention: m (or X) down, n (or Y) to right
% properly handles situation when one or both images contain some NaN points

function [p1,p2,p3,p4,p5,p6,p7,p8,p9,p10]=f(E1,E2); % 1 or 10 returns

%%%%mask = isnan(E1).*isnan(E2);  % NaN mask: regions where either is undefined
% doesnt work because nanmask makes it still nan

[El,Em,En] = derivatives(E1,E2);
[M,N]= size(El);                    % size of any of the 3 derivative matrices
[n m]=meshdom(1:N,M:-1:1);          % variables ``x'' and ``y''

DERIVATIVES = ... % must indent; if you do not indent then the )...sum2nan will not work, you need ) ...sum2nan (e.g. the space)
 [...
  [sum2nan(Em       .*Em) sum2nan(m.*Em       .*Em) sum2nan(n.*Em       .*Em) sum2nan(m.*m.*Em       .*Em) sum2nan(m.*n.*Em       .*Em)...
     sum2nan(En       .*Em) sum2nan(m.*En       .*Em) sum2nan(n.*En       .*Em) sum2nan(n.*n.*En       .*Em) sum2nan(m.*n.*En       .*Em)];...
  [sum2nan(Em    .*m.*Em) sum2nan(m.*Em    .*m.*Em) sum2nan(n.*Em    .*m.*Em) sum2nan(m.*m.*Em    .*m.*Em) sum2nan(m.*n.*Em    .*m.*Em)...
     sum2nan(En    .*m.*Em) sum2nan(m.*En    .*m.*Em) sum2nan(n.*En    .*m.*Em) sum2nan(n.*n.*En    .*m.*Em) sum2nan(m.*n.*En    .*m.*Em)];...
  [sum2nan(Em    .*n.*Em) sum2nan(m.*Em    .*n.*Em) sum2nan(n.*Em    .*n.*Em) sum2nan(m.*m.*Em    .*n.*Em) sum2nan(m.*n.*Em    .*n.*Em)...
     sum2nan(En    .*n.*Em) sum2nan(m.*En    .*n.*Em) sum2nan(n.*En    .*n.*Em) sum2nan(n.*n.*En    .*n.*Em) sum2nan(m.*n.*En    .*n.*Em)];...
  [sum2nan(Em .*m.*m.*Em) sum2nan(m.*Em .*m.*m.*Em) sum2nan(n.*Em .*m.*m.*Em) sum2nan(m.*m.*Em .*m.*m.*Em) sum2nan(m.*n.*Em .*m.*m.*Em)...
     sum2nan(En .*m.*m.*Em) sum2nan(m.*En .*m.*m.*Em) sum2nan(n.*En .*m.*m.*Em) sum2nan(n.*n.*En .*m.*m.*Em) sum2nan(m.*n.*En .*m.*m.*Em)];...
  [sum2nan(Em .*m.*n.*Em) sum2nan(m.*Em .*m.*n.*Em) sum2nan(n.*Em .*m.*n.*Em) sum2nan(m.*m.*Em .*m.*n.*Em) sum2nan(m.*n.*Em .*m.*n.*Em)...
     sum2nan(En .*m.*n.*Em) sum2nan(m.*En .*m.*n.*Em) sum2nan(n.*En .*m.*n.*Em) sum2nan(n.*n.*En .*m.*n.*Em) sum2nan(m.*n.*En .*m.*n.*Em)];...
  [sum2nan(Em       .*En) sum2nan(m.*Em       .*En) sum2nan(n.*Em       .*En) sum2nan(m.*m.*Em       .*En) sum2nan(m.*n.*Em       .*En)...
     sum2nan(En       .*En) sum2nan(m.*En       .*En) sum2nan(n.*En       .*En) sum2nan(n.*n.*En       .*En) sum2nan(m.*n.*En       .*En)];...
  [sum2nan(Em    .*m.*En) sum2nan(m.*Em    .*m.*En) sum2nan(n.*Em    .*m.*En) sum2nan(m.*m.*Em    .*m.*En) sum2nan(m.*n.*Em    .*m.*En)...
     sum2nan(En    .*m.*En) sum2nan(m.*En    .*m.*En) sum2nan(n.*En    .*m.*En) sum2nan(n.*n.*En    .*m.*En) sum2nan(m.*n.*En    .*m.*En)];...
  [sum2nan(Em    .*n.*En) sum2nan(m.*Em    .*n.*En) sum2nan(n.*Em    .*n.*En) sum2nan(m.*m.*Em    .*n.*En) sum2nan(m.*n.*Em    .*n.*En)...
     sum2nan(En    .*n.*En) sum2nan(m.*En    .*n.*En) sum2nan(n.*En    .*n.*En) sum2nan(n.*n.*En    .*n.*En) sum2nan(m.*n.*En    .*n.*En)];...
  [sum2nan(Em .*n.*n.*En) sum2nan(m.*Em .*n.*n.*En) sum2nan(n.*Em .*n.*n.*En) sum2nan(m.*m.*Em .*n.*n.*En) sum2nan(m.*n.*Em .*n.*n.*En)...
     sum2nan(En .*n.*n.*En) sum2nan(m.*En .*n.*n.*En) sum2nan(n.*En .*n.*n.*En) sum2nan(n.*n.*En .*n.*n.*En) sum2nan(m.*n.*En .*n.*n.*En)];...
  [sum2nan(Em .*m.*n.*En) sum2nan(m.*Em .*m.*n.*En) sum2nan(n.*Em .*m.*n.*En) sum2nan(m.*m.*Em .*m.*n.*En) sum2nan(m.*n.*Em .*m.*n.*En)...
     sum2nan(En .*m.*n.*En) sum2nan(m.*En .*m.*n.*En) sum2nan(n.*En .*m.*n.*En) sum2nan(n.*n.*En .*m.*n.*En) sum2nan(m.*n.*En .*m.*n.*En)];...
 ];

Derivatives = ...
 - [sum2nan(El       .*Em); ...
    sum2nan(El    .*m.*Em); ...
    sum2nan(El    .*n.*Em); ...
    sum2nan(El .*m.*m.*Em); ...
    sum2nan(El .*m.*n.*Em); ...
    sum2nan(El       .*En); ...
    sum2nan(El    .*m.*En); ...
    sum2nan(El    .*n.*En); ...
    sum2nan(El .*n.*n.*En); ...
    sum2nan(El .*m.*n.*En) ...
   ];                         % vector of derivatives

parameters = DERIVATIVES\Derivatives;
disp('est_qchirp2: adding identity and converting from pel to normalized [0,1)')
parameters(:) = parameters(:) + [0;1;0;0;0; 0;0;1;0;0];  % convert relative change like u=func(x+deltax) to absolute like u=func(x)
parameters = parameters(:) .* [1/M;1;N/M;M;N; 1/N;M/N;1;N;M];

if (nargout == 10) | (nargout == 0)
  % p1 = parameters(1);           % if nargin==0 must never make ref to p1
  p2 = parameters(2);             % or you end up with ans=[], when called with
  p3 = parameters(3);             % no semicolon
  p4 = parameters(4);
  p5 = parameters(5);
  p6 = parameters(6);
  p7 = parameters(7);
  p8 = parameters(8);
  p9 = parameters(9);
  p10= parameters(10);
end%if
if nargout == 10
  p1 = parameters(1);
end%if

if nargout == 1
  p1 = parameters;
end%if

if nargout == 0   % no output argument
  disp('est_qchirp2: 0 output arguments given so I am displaying to screen:')
  %disp(sprintf('%g %g %g %g %g %g',p1,p2,p3,p4,p5))
  disp(sprintf('%g %g %g %g %g',parameters(1),p2,p3,p4,p5))
  disp(sprintf('%g %g %g %g %g',p6,p7,p8,p9,p10))
end%if

if (nargout ~= 0) & (nargout ~= 1) & (nargout ~= 10)
  error('est_qchirp2: you must have 0, 1, or 10 output arguments')
end%if