Minpack.hh

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
  Multimediale Systeme der Informationsverarbeitung
  Institut fuer Informatik
  Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS 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 Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/


#ifndef __Minpack_hh__
#define __Minpack_hh__

#include <bias_config.h>

#include <Base/Math/Matrix.hh>
#include <Base/Math/Vector.hh>
#include <MathAlgo/minpack/cminpack.h>

#include "Lapack.hh"

#define MINPACK_DEF_TOLERANCE 1e-7
#define LM_DEF_MAX_ITER -1
#define LM_DEF_EPSILON 1e-15

#define LM_IMPROPER_INPUT -1
#define LM_OK 0
#define LM_TOO_MANY_IT 5
#define LM_SMALL_FTOL  6.
#define LM_SMALL_XTOL  7
#define LM_SMALL_GTOL  8

namespace BIAS
{
/** @addtogroup g_mathalgo
    \brief C++ Wrappers for important Lapack fortran77 functions
    @{
*/

  /** Solve a system f(x) = 0 of n non-linear equations with n unknowns using the
      Powell hybrid method. The Jacobian matrix J(x) is calculated numerically by
      forward-difference approximation. The error is assumed to be within the
      range of machine precision.

      @param fun is the name of the user-supplied subroutine which calculates
      the target function f at x and returns the values in array fvec.
      fun must be declared e.g. in the user calling program and have the following
      interface:

      int fun(void *p, int n, const double *x, double *fvec, int iflag)

      - p points to an object with additional data used for evaluating f(x)
      - n is a positive integer input variable set to the number of residuals
        and parameters (i.e. size of array x and fvec).
      - x is the parameter array of length n. On input, x must contain an
        initial estimate of the solution vector. On output, x contains the final
        estimate of the solution vector x_res.
      - fvec is an output array of length n which contains the function values
        f(x) evaluated at the input parameter vector x.
      - iflag should not be changed by fun unless the user wants to terminate
        execution, in this case set iflag to a negative integer.

      @param Tolerance is a non-negative input variable. Termination occurs when
      the algorithm estimates that the relative error between x and the solution x_res
      is at most Tolerance.

      @return 0 if everything worked fine. If the user has terminated execution by
      setting iflag to < 0 in fun, the value of iflag is returned. Otherwise, return is:
      - return = -1: Improper input parameters
      - return =  0: Algorithm estimated that the relative error between x and the
                     solution is within tolerance range.
      - return =  2: Number of calls to fun has reached or exceeded 200*(n+1).
      - return =  3: Tolerance is too small. No further improvement in the approximate
                     solution x is possible.
      - return =  4: Iteration is not making good progress.

      @see minpack/hybrd1.f
      @author woelk
  */
  BIASMathAlgo_EXPORT
  long int Powell(minpack_func_nn fun, void *p,
                  Vector<double>& InitialGuess, Vector<double>& Result,
                  double Tolerance = MINPACK_DEF_TOLERANCE);

  /** Solve a system f(n) = 0 of n non-linear equations with n unknowns using the
      Powell hybrid method. The Jacobian matrix J(x) is approximated numerically by
      forward-difference approximation.

      See description of method Powell above for further parameter documentation.

      @param EpsilonFun Non-negative step width used to approximate the Jacobian numerically.
      
      @see minpack/hybrd.f
      @author woelk
  */
  BIASMathAlgo_EXPORT
  long int PowellExtended(minpack_func_nn fun, void *p,
                          Vector<double>& InitialGuess, Vector<double>& Result,
                          double EpsilonFun = LM_DEF_EPSILON,
                          double Tolerance = MINPACK_DEF_TOLERANCE);

  /** Solves an overdetermined system f(x) = 0 of m non-linear equations with n
      parameters using the Levenberg-Marquardt algorithm. A least squares
      solution is used. Jacobian J(x) of target function f(x) is given analytically.

      @param fun Defines the target function f(x) (resp. the Jacobian matrix J(x))
      which is supposed to have the following interface:

      int fun(void *p, int m, int n, const double *x, double *fvec,
              double *fjac, int ldfjac, int iflag)

      - p points to an object with additional data used for evaluating f(x)
      - m is number of residuals (size of array fvec)
      - n is number of parameters (size of array x)
      - x points to an array of size n containing the input parameter values
      - fvec points to an array of size m where the output residuals f(x)
        are written to
      - fjac points to an array of size m*n where the elements of the Jacobian
        matrix J(x) are written to
      - ldfjac specifies the leading dimension used to describe the Jacobian
        matrix J(x) by the array fjac (if ldfjac = m then J(x) is written
        column-wise into array fjac).
      - iflag is a flag that specifies if the function should compute the
        residuals f(x) and write them to fvec (when iflag == 1), or if the
        Jacobian matrix J(x) should be computed and written to fjac (when
        iflag == 2). The value of iflag should not be changed!

      @param m Number of residuals f(x) with m>=n
      @param n Number of variables x with m>=n
      @param InitialGuess Starting point x_0 for optimization (input, size must be n)
      @param Result Solution x_res after optimization (output, size must be n)

      @param Tolerance Non-negative threshold used to determine convergence.

      @return 0 determines success in optimization, other return values are:
      - return = -1: Improper input parameters
      - return =  0: A termination condition became true.
      - return =  5: Number of calls to fun has reached or exceeded maxfev iterations.
      - return =  6: Tolerance ftol is too small. No further reduction in sum of
                     residual squares possible.
      - return =  7: Tolerance xtol is too small. No further improvement in
                     approximate solution x is possible.
      - return =  8: Tolerance gtol is too small. Vector fvec is orthogonal to the
                     columns of the Jacobian to machine precision.

      @see minpack/lmder1.f
      @author woelk
  */
  BIASMathAlgo_EXPORT
  long int LevenbergMarquardt(minpack_funcder_mn fun,
                              void *p,
                              long int m,
                              long int n,
                              Vector<double>& InitialGuess,
                              Vector<double>& Result,
                              double Tolerance = MINPACK_DEF_TOLERANCE);

  /** Solves an overdetermined system f(x) = 0 of m non-linear equations with n
      parameters using the extended Levenberg-Marquardt algorithm. A least squares
      solution is used. Jacobian J(x) of target function f(x) is given analytically.

      @param fun Defines the target function f(x) (resp. the Jacobian matrix J(x))
      which is supposed to have the following interface:

      int fun(void *p, int m, int n, const double *x, double *fvec,
              double *fjac, int ldfjac, int iflag)

      - p points to an object with additional data used for evaluating f(x)
      - m is number of residuals (size of array fvec)
      - n is number of parameters (size of array x)
      - x points to an array of size n containing the input parameter values
      - fvec points to an array of size m where the output residuals f(x)
        are written to
      - fjac points to an array of size m*n where the elements of the Jacobian
        matrix J(x) are written to
      - ldfjac specifies the leading dimension used to describe the Jacobian
        matrix J(x) by the array fjac (if ldfjac = m then J(x) is written
        column-wise into array fjac).
      - iflag is a flag that specifies if the function should compute the
        residuals f(x) and write them to fvec (when iflag == 1), or if the
        Jacobian matrix J(x) should be computed and written to fjac (when
        iflag == 2). The value of iflag should not be changed!

      @param m Number of residuals f(x) with m>=n
      @param n Number of variables x with m>=n
      @param InitialGuess Starting point x_0 for optimization (input, size must be n)
      @param Result Solution x_res after optimization (output, size must be n)

      @param Tolerance Non-negative threshold used to determine convergence.

      @param MaxIter Max. number of evaluations of target function f(x). Note that
      the complete numerical approximation of the Jacobian J(x) is always done as a
      whole before this value is checked. Set MaxIter to -1 if you want to use the
      default number of max. iterations from the fortran code (= 200*(n+1)).

      @return 0 determines success in optimization, for other return see description
      for method LevenbergMarquardt above.
      
      @see minpack/lmder.f
      @author woelk
  */
  BIASMathAlgo_EXPORT
  long int LevenbergMarquardtExtended(minpack_funcder_mn fun,
                                      void *p,
                                      long int m,
                                      long int n,
                                      Vector<double>& InitialGuess,
                                      Vector<double>& Result,
                                      double Tolerance = MINPACK_DEF_TOLERANCE,
                                      long int MaxIter = LM_DEF_MAX_ITER);

  /** Solves an overdetermined system f(x) = 0 of m non-linear equations with n
      parameters using the Levenberg-Marquardt algorithm. A least squares
      solution is used. The Jacobian J(x) of target function f(x) is calculated
      numerically by forward-difference approximation and must not be provided
      analytically.

      @param fun Defines the target function f(x) which is supposed to have the
      following interface:

      int fun(void *p, int m, int n, const double *x, double *fvec, int iflag)

      - p points to an object with additional data used for evaluating f(x)
      - m is number of residuals (size of array fvec)
      - n is number of parameters (size of array x)
      - x points to an array of size n containing the input parameter values
      - fvec points to an array of size m where the output residuals f(x)
        are written to
      - iflag can be ignores here

      @param m Number of residuals f(x) with m>=n
      @param n Number of variables x with m>=n
      @param InitialGuess Starting point x_0 for optimization (input, size must be n)
      @param Result Solution x_res after optimization (output, size must be n)

      @param Tolerance Non-negative threshold used to determine convergence.

      @param MaxIter This parameter is ignored and should be removed!

      @return 0 determines success in optimization, for other return see description
      for method LevenbergMarquardt above.
      
      @see minpack/lmdif1.f
      @author woelk
  */
  BIASMathAlgo_EXPORT
  long int LevenbergMarquardt(minpack_func_mn fun,void *p,
                              long int M,
                              long int N,
                              Vector<double>& InitialGuess,
                              Vector<double>& Result,
                              double Tolerance = MINPACK_DEF_TOLERANCE,
                              long int MaxIter = LM_DEF_MAX_ITER);

  /** Solves an overdetermined system f(x) = 0 of m non-linear equations with n
      parameters using the Levenberg-Marquardt algorithm. A least squares
      solution is used. The Jacobian J(x) of target function f(x) is calculated
      numerically by forward-difference approximation and must not be provided
      analytically.

      @param fun Defines the target function f(x) which is supposed to have the
      following interface:

      int fun(void *p, int m, int n, const double *x, double *fvec, int iflag)

      - p points to an object with additional data used for evaluating f(x)
      - m is number of residuals (size of array fvec)
      - n is number of parameters (size of array x)
      - x points to an array of size n containing the input parameter values
      - fvec points to an array of size m where the output residuals f(x)
        are written to
      - iflag can be ignores here

      @param m Number of residuals f(x) with m>=n
      @param n Number of variables x with m>=n
      @param InitialGuess Starting point x_0 for optimization (input, size must be n)
      @param Result Solution x_res after optimization (output, size must be n)

      @param EpsilonFun Non-negative step width used to approximate the Jacobian numerically.

      @param Tolerance Non-negative threshold used to determine convergence.

      @param MaxIter Max. number of evaluations of target function f(x) (-1: default).

      @param ResultingJacobian Pointer to array of size m*n to write the Jacobian J(x_res)
      evaluated numerically at the solution x_res. Pass NULL if it should be omitted.

      @param ipvt If Jacobian J(x_res) should be returned in ResultingJacobian,
      you must specify here a pointer to an integer array of size n to write the
      permutation matrix P to. Otherwise pass NULL.

      P is computed so that J(x_res)*P = Q*R, where J(x_res) is the final calculated
      Jacobian, Q is an orthogonal matrix, and R is an upper triangular matrix with
      diagonal elements of nonincreasing magnitude. Column j of P is column ipvt[j]
      of the identity matrix.
      Search minpack/minpack-documentation.txt for IPVT for a detailed description
      of this parameter!

      @return 0 determines success in optimization, for other return see description
      for method LevenbergMarquardt above.
      
      @see minpack/lmdif.f
      @author woelk
  */
  BIASMathAlgo_EXPORT
  long int LevenbergMarquardtExtended(minpack_func_mn fun,
                                      void *p,
                                      long int M,
                                      long int N,
                                      Vector<double>& InitialGuess,
                                      Vector<double>& Result,
                                      double EpsilonFun = LM_DEF_EPSILON,
                                      double Tolerance = MINPACK_DEF_TOLERANCE,
                                      long int MaxIter = LM_DEF_MAX_ITER,
                                      double* ResultingJacobian = NULL,
                                      int* ipvt = NULL,
                                      double *SumOfSquaredErrors = NULL);

  /** @brief Compute covariance matrix from Levenberg-Marquardt resulting Jacobian matrix
      J(x) and permutation matrix P (see description for method LevenbergMarquardtExtended
      above or search in minpack/minpack-documentation.txt for IPVT).

      @param M Number of residuals (= rows of Jacobian)
      @param N Number of parameters (= columns of Jacobian)
      @param ResultingJacobian Pointer to array of size M*N containing the resulting
             Jacobian J(x_res) computed from call to ExtendedLevenbergMarquardt.
      @param ipvt Pointer to integer array of size N describing the permutation matrix
             P computed from call to ExtendedLevenbergMarquardt (i.e. J(x_res)*P = Q*R).
      @param SumOfSquaredErrors Sum of squared errors computed by ExtendedLevenbergMarquardt
      @param Cov Output matrix to store covariance in

      @author woelk 06/2006
  */
  BIASMathAlgo_EXPORT
  long int ComputeCovariance(long int M, long int N, double *ResultingJacobian,
                             int *ipvt, double &SumOfSquaredErrors,
                             Matrix<double>& Cov);

  /** @brief Convenience wrapper for ComputeCovariance.

      Compute covariance matrix from Levenberg-Marquardt resulting Jacobian matrix
      J(x) and permutation matrix P (see description for method LevenbergMarquardtExtended
      above or search in minpack/minpack-documentation.txt for IPVT).

      @param NumErrors Number of residuals (= rows of Jacobian)
      @param NumParams Number of parameters (= columns of Jacobian)
      @param Jac Matrix of size NumErrors x NumParams containing the resulting
             Jacobian J(x_res) computed from ExtendedLevenbergMarquardt.
      @param Permutation Integer vector of size NumParams describing the permutation matrix
             P computed from ExtendedLevenbergMarquardt (i.e. J(x_res)*P = Q*R).
      @param SumOfSquaredErrors Sum of squared errors computed by ExtendedLevenbergMarquardt
      @param Cov Output matrix to store covariance int

      @author koeser
  */
  BIASMathAlgo_EXPORT
  long int ComputeCovariance(const long int NumErrors, const long int NumParams,
                             const Matrix<double> &Jac,
                             const Vector<int>& Permutation,
                             const double &SumOfSquaredErrors,
                             Matrix<double>& Cov);

  /** @brief Convenience function to check correctness of user supplied Jacobians
      J(x) needed by the analytical version of the Levenberg-Marquardt algorithm.

      First, the user defined Jacobian J(x) of f at x is calculated by calling fun
      with iflag = 2. Then the minpack function chkder is used to evaluate the Jacobian.

      @warning CheckJacobian does not perform reliably if cancellation or
      rounding errors cause a severe loss of significance in the evaluation
      of a function!

      @param fun The target function fun is supposed to have the same interface
      as described above in LevenbergMarquardt (version WITH Jacobian computation):

      int fun(void *p, int m, int n, const double *x, double *fvec,
              double *fjac, int ldfjac, int iflag)

      @param p points to an object with additional data used for evaluating J(x)
      @param NumErrors is the number m of residuals of f(x) (i.e. the number of rows of J(x))
      @param x is a vector of size n that defines where the Jacobian J(x) is evaluated at

      @param error The function returns an error measure for each row of the Jacobian J(x)
      in the output vector error of size NumErrors.

      On output, error contains measures of correctness of the respective gradients.
      If there is no severe loss of significance, then if error[i] is 1.0, the i-th gradient
      is correct, while if error[i] is 0.0, the i-th gradient is incorrect. For values
      of error[i] between 0.0 and 1.0, the categorization is less certain.
      In general, a value of error[i] > 0.5 indicates that the i-th gradient is probably
      correct, while a value of error[i] < 0.5 indicates that the i-th gradient is probably
      incorrect.

      @author woelk 07/2006
  */
  BIASMathAlgo_EXPORT
  void CheckJacobian(minpack_funcder_mn fun, void *p,
                     const long int NumErrors, const Vector<double> &x,
                     Vector<double> &error);

  /** @brief Compute the Jacobian J(x) of function f at x.
      @param fun specifies the target function f(x) to compute the Jacobian J(x) for
      @param p points to an object with additional data used for evaluating J(x)
      @param x is a vector of size n that defines where the Jacobian J(x) is evaluated at
      @param Jac is an output matrix to which the Jacobian J(x) of function f evaluated
             at x is written to
      @note Wrapper for fdjac2
      @see minpack/fdjac2.c
      @author woelk 07/2007
  */
  BIASMathAlgo_EXPORT
  long int ComputeJacobian(minpack_func_mn fun, void *p,
                           const int NumErrors, const Vector<double> &x,
                           Matrix<double>& Jac,
                           const double gradientEpsilon = LM_DEF_EPSILON);

  /** @brief Compute the QR decomposition of input matrix A using Householder
             transformations without column pivoting.
      @param[in]  A is the input matrix which will be QR decomposed.
      @param[out] Q is the resulting Q matrix which is orthogonal (A = Q*R)
      @param[out] R is the resulting R matrix which is upper trapezoidal (A = Q*R)
      @note Wrapper for qrfac
      @see minpack/qrfac.c
  */
  BIASMathAlgo_EXPORT
  void QRFrac(const Matrix<double> &A, Matrix<double>& Q, Matrix<double>& R);
  // @}
} // namespace

#endif // __Minpack_hh__