## Copyright (C) 2012 pdiribarne
## 
## This program is free software; you can redistribute it and/or modify
## it under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 3 of the License, or
## (at your option) any later version.
## 
## This program is distributed in the hope that it will be useful,
## but WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
## GNU General Public License for more details.
## 
## You should have received a copy of the GNU General Public License
## along with Octave; see the file COPYING.  If not, see
## <http://www.gnu.org/licenses/>.

## -*- texinfo -*-
## @deftypefn  {Function File} {@var{T} = } cp2tform (@var{ap_pt},@var{rw_pt}, @var{transtype})
## @deftypefnx {Function File} {@var{T} = } cp2tform (@var{ap_pt},@var{rw_pt}, @var{transtype}, @var{opt})
## Returns a transformation structure @var{T} which can be further used
## to transform apparent 2 dimensionnal coordinates @var{ap_pt} (m x 2
## matrix) into real world coordinates @var{rw_pt}. The transform
## structure contains fields 'ndims_in', 'ndims_out', 'forward_fcn',
## 'inverse_fcn' and 'tdata'. Transform coefficients @var{Tcoefs} are stored
## in @var{T}.tdata.T. Interpretation of transform coefs depends on the
## requested transform type @var{transtype}:
##
## @table @asis
## @item "affine"
## Denoting x and y the apparent coordinates vector and xrw, yrw the
## the real world coordinates. @var{Tcoefs} is a 3x2 matrix which  can
## be used as follows:
##
## @example
## @group
## [xrw yrw] = [x y 1] * @var{Tcoefs}
## @end group
## @end example
##
## @item "polynomial"
## Here the @var{opt} input argument is the order of the polynomial
## fit. @var{opt} must be 2, 3 or 4 and input control points number must
## be respectively at least 6, 10 and 15.
## Denoting x and y the apparent coordinates vector and xrw, yrw the
## the real world coordinates. @var{Tcoefs} is a (6 or 10 or 15)x2
## matrix which can be used as follows:
##
## @example
## @group
## Second order:  
## [xrw yrw] = [1 x y x*y x^2 y^2] * @var{Tcoefs}
## @end group
## @group
## Third order:   
## [xrw yrw] = [1 x y x*y x^2 y^2 y*x^2 x*y^2 x^3 y^3] * @var{Tcoefs}
## @end group
## @group
## Fourth order:  
## [xrw yrw] = [1 x y x*y x^2 y^2 y*x^2 x*y^2 x^3 y^3 x^3*y x^2*y^2 x*y^3 x^4 y^4] * @var{Tcoefs}
## @end group
## @end example
## @end table
## @end deftypefn

## Author: pdiribarne <pdiribarne@mac-diribarne.intra.cea.fr>
## Created: 2012-07-25

function varargout = cp2tform (varargin)
  if (nargin < 3)
    print_usage ();
  endif

  if (! all (size (varargin{1}) == size (varargin{2})) ||
      columns (varargin{1}) != 2)
    error ("cp2tform: expect the 2 first input arguments to be (m x 2) matrices")
  elseif (! ischar (varargin{3}))
    error ("cp2tform: expect a string as third input argument")
  endif

  cap = varargin{1};
  crw = varargin{2};
  ttype = lower (varargin{3});

  switch ttype
    case 'affine'
      trans = gettrans (ttype, cap, crw);
      varargout{1} = trans;
    case 'polynomial'
      if (nargin < 4)
        error ("cp2tform: expect a fourth input argument for 'polynomial'")
      elseif (! isscalar (varargin{4}))
        error ("cp2tform: expect a scalar as fourth argument")
      endif
      ord = round (varargin{4});
      trans = gettrans (ttype, cap, crw, ord);
      varargout{1} = trans;
    otherwise
      error ("cp2tform: expect 'affine' or 'polynomial' as third input argument")
  endswitch  
endfunction

function trans = gettrans (ttype, cap, crw, ord)
  if (strcmp (ttype, 'affine'))
    trans.ndims_in = 2;
    trans.ndims_out = 2;
    trans.forward_fcn = @affin;
    trans.inverse_fcn = [];
    leasqrfunc = @affin; 
    leasqrdfdp = @daffin; 
    pin = ones (3, 1);
  elseif (strcmp (ttype, 'polynomial'))
    trans.ndims_in = 2;
    trans.ndims_out = 2;
    trans.forward_fcn = [];
    trans.inverse_fcn = [];
    switch ord
      case 2
        leasqrfunc = @poly2; 
        leasqrdfdp = @dpoly2; 
        pin = ones (6, 1);
        trans.forward_fcn = @poly2;
      case 3
        leasqrfunc = @poly3; 
        leasqrdfdp = @dpoly3; 
        pin = ones (10, 1);
        trans.forward_fcn = @poly3;
      case 4
        leasqrfunc = @poly4; 
        leasqrdfdp = @dpoly4; 
        pin = ones (15, 1);
        trans.forward_fcn = @poly4;
      otherwise
        error ("only order 2 is implemented")
    endswitch
  endif
    
  dp = 1e-3 * ones (size (pin));
  minstep = 1e-4 * ones (size (pin));
  maxstep = 1e6 * ones (size (pin));
  options = [minstep, maxstep];
  
  wt1 = ones (rows (crw), 1);
  stol=0.0001;
  niter=50;

  
  verb = isempty (who ("verbose", "global"));
  if verb
    global verbose
    verbon = verbose;
    verbose = 0;
  endif
  try 
    [f1, p1] = leasqr (cap, crw(:,1), pin, leasqrfunc,
                       stol, niter, wt1, dp, leasqrdfdp, options);
    [f1, p2] = leasqr (cap, crw(:,2), pin, leasqrfunc,
                       stol, niter, wt1, dp, leasqrdfdp, options);
  catch
    verbose = verbon;
    error ("cp2tform: %s", lasterr)
  end_try_catch

  if verb
    verbose = verbon;
  endif
  trans.tdata = [p1 p2];
  
endfunction

## fitting functions F(x,p) and corresponding dF/dp
function out = affin (x, p)
  out = p(3) + p(1)*x(:,1) + p(2)*x(:,2);
endfunction

function out = daffin (x, f)
  out = [x(:,1), x(:,2), ones(rows(x), 1)];
endfunction

function out = poly2 (x, p)
  out = p(1) + p(2)*x(:,1) + p(3)*x(:,2) + p(4)*x(:,1).*x(:,2) + \
        p(5)*x(:,1).^2 + p(6)*x(:,2).^2;
endfunction

function out = dpoly2 (x, f)
  out = [ones(rows(x), 1), x(:,1), x(:,2), ...
         x(:,1).*x(:,2), x(:,1).^2, x(:,2).^2];
endfunction

function out = poly3 (x, p)
  out = p(1) + p(2)*x(:,1) + p(3)*x(:,2) + p(4)*x(:,1).*x(:,2) + \
        p(5)*x(:,1).^2 + p(6)*x(:,2).^2 + p(7)*x(:,2).*x(:,1).^2 + \
        p(8)*x(:,1).*x(:,2).^2 + p(9)*x(:,1).^3 + p(10)*x(:,2).^3;
endfunction

function out = dpoly3 (x, f)
  out = [ones(rows(x), 1), x(:,1), x(:,2), x(:,1).*x(:,2), ...
        x(:,1).^2, x(:,2).^2, x(:,2).*x(:,1).^2, ...
        x(:,1).*x(:,2).^2, x(:,1).^3, x(:,2).^3];
endfunction

function out = poly4 (x, p)
  out = p(1) + p(2)*x(:,1) + p(3)*x(:,2) + p(4)*x(:,1).*x(:,2) + \
        p(5)*x(:,1).^2 + p(6)*x(:,2).^2 + p(7)*x(:,2).*x(:,1).^2 + \
        p(8)*x(:,1).*x(:,2).^2 + p(9)*x(:,1).^3 + p(10)*x(:,2).^3 + \
        p(11)*x(:,2).*x(:,1).^3 + p(12)*x(:,2).^2.*x(:,1).^2+ \
        p(13)*x(:,1).*x(:,2).^3 + p(14)*x(:,1).^4 + p(15)*x(:,2).^4;
endfunction

function out = dpoly4 (x, f)
  out = [ones(rows(x), 1), x(:,1), x(:,2), x(:,1).*x(:,2), ...
        x(:,1).^2, x(:,2).^2, x(:,2).*x(:,1).^2, ...
        x(:,1).*x(:,2).^2, x(:,1).^3, x(:,2).^3, ...
        x(:,2).*x(:,1).^3, x(:,2).^2.*x(:,1).^2, ...
        x(:,1).*x(:,2).^3, x(:,1).^4, x(:,2).^4];
endfunction
