LevenbergMarquardtBase.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 _LevenbergMarquardtBase_hh_
#define _LevenbergMarquardtBase_hh_

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

namespace BIAS {

/** @class LevenbergMarquardtBase
    @brief Base interface for classes using non-linear optimization of
           a target function using the Levenberg-Marquardt algorithm.

    @note Since this class used virtual function calls to evaluate the
          target function of the Levenber-Marquardt instance, and data
          is copied between BIAS data structure and arrays in every
          iteration, it is not well suited for high-performance demands!
          Use the direct Minpack interface from Minpack.hh instead when
          you are targeting at performance!

    Provides a class interface to Minpack with convenient definition of
    target function f(x) and Jacobian J(x).
    @see Minpack.hh

    @author esquivel
    @date 05/2010
*/
    
  class BIASMathAlgo_EXPORT LevenbergMarquardtBase
  {

  private:

    /// @brief Tolerance for Levenberg-Marquardt optimization
    double LM_tolerance_;

    /// @brief Max. number of iterations for Levenberg-Marquardt algorithm
    int LM_maxIters_;

    /// @brief Variables to store results of current computation step
    double LM_squaredSum_;
    int LM_numIters_;
    BIAS::Vector<double> LM_parameters_;
    BIAS::Vector<double> LM_residuals_;
    BIAS::Matrix<double> LM_Jacobian_;

  public:

    LevenbergMarquardtBase()
    {
      LM_tolerance_ = MINPACK_DEF_TOLERANCE;
      LM_maxIters_ = LM_DEF_MAX_ITER;
      LM_squaredSum_ = 0;
      LM_numIters_ = 0;
    }

    virtual ~LevenbergMarquardtBase() {}

  protected:

    /// @brief Compute residuals of target function f for input vector x.
    /// @note Override this with the user specified target function.
    virtual int LM_TargetFunction_(const BIAS::Vector<double> &x,
                                   BIAS::Vector<double> &residuals) = 0;

    /// @brief Compute Jacobian J(x) of target function f evaluated at vector x
    /// @note Override this with the user specified Jacobian function.
    ///       Otherwise, only a numerical approximation of J(x) will be used.
    virtual int LM_JacobianFunction_(const BIAS::Vector<double> &x,
                                     BIAS::Matrix<double> &J)
    {
      BIASERR("No analytic Jacobian defined for target function!");
      BIASABORT;
      return 0; // added to appease windows compiler 
    }

  public:

    /// @brief Set tolerance used to evaluate convergence
    inline void LM_SetTolerance(const double tol)
    { LM_tolerance_ = (tol > 0 ? tol : MINPACK_DEF_TOLERANCE); }

    /// @brief Return tolerance used to evaluate convergence
    inline double LM_GetTolerance() const
    { return LM_tolerance_; }

    /// @brief Set the maximal number of iterations to perform
    inline void LM_SetMaxIterations(const int iters)
    { LM_maxIters_ = (iters > 0 ? iters : LM_DEF_MAX_ITER); }

    /// @brief Return the maximal number of iterations to perform
    inline int LM_GetMaxIterations() const
    { return LM_maxIters_; }

    /// @brief Return the sum of squared errors from the last computation
    inline double LM_GetSumOfSquaredErrors() const
    { return LM_squaredSum_; }

    /// @brief Return the number of iterations performed in last computation
    inline int LM_GetNumOfIterations() const
    { return LM_numIters_; }

    /// @brief Return the resulting parameter vector x from last computation
    inline BIAS::Vector<double> LM_GetParameterResult() const
    { return LM_parameters_; }

    /// @brief Return the resulting residual vector f(x) from last computation
    inline BIAS::Vector<double> LM_GetResidualsResult() const
    { return LM_residuals_; }

    /// @brief Return the resulting Jacobian J(x) from the last computation
    inline BIAS::Matrix<double> LM_GetJacobianResult() const
    { return LM_Jacobian_; }

    /** @brief Compute Levenberg-Marquardt algorithm using the user defined
               target function f(x) implemented in LM_TargetFunction_,
               and the analytic Jacobian J(x) implemented in LM_Jacobian_.
        @return Returns value < 0 if computation failed, 0 for convergence,
                and > 0 otherwise (see #LevenbergMarquardt in #Minpack.hh).
    */
    int LM_Compute(const int numResiduals, const int numParameters,
                   BIAS::Vector<double> &startX, BIAS::Vector<double> &resultX,
                   BIAS::Vector<double> &residuals);

    /** @brief Compute Levenberg-Marquardt algorithm using the user defined
               target function f(x) implemented in LM_TargetFunction_.
               The Jacobian of f is approximated numerically using the given
               value epsilon.
        @param epsilon is "an input variable used in determining a suitable
               step length for the forward-difference approximation" of the
               Jacobian of f (see documentation of minpack/fdjac2.c)
        @return Returns value < 0 if computation failed, 0 for convergence,
                and > 0 otherwise (see #LevenbergMarquardt in #Minpack.hh).
    */
    int LM_ComputeWithoutJacobian(const int numResiduals,
                                  const int numParameters,
                                  BIAS::Vector<double> &startX,
                                  BIAS::Vector<double> &resultX,
                                  BIAS::Vector<double> &residuals,
                                  const double epsilon = LM_DEF_EPSILON);

  private:

    /// @brief Static target function with derivatives used by minpack
    inline static int g_MinpackFunction(void *ptr, int m, int n,
                                        const double *x,
                                        double *fvec, double *fjac,
                                        int ldfjac, int iflag)
    {
      LevenbergMarquardtBase *LM = static_cast<LevenbergMarquardtBase *>(ptr);
      BIASASSERT(LM);
      return LM->MinpackFunction_(m, n, x, fvec, fjac, ldfjac, iflag);
    }

    /// @brief Static target function w/o derivatives used by minpack
    inline static int g_MinpackFunctionWithoutJacobian(void *ptr, int m, int n,
                                                       const double *x,
                                                       double *fvec, int iflag)
    {
      LevenbergMarquardtBase *LM = static_cast<LevenbergMarquardtBase *>(ptr);
      BIASASSERT(LM);
      return LM->MinpackFunctionWithoutJacobian_(m, n, x, fvec, iflag);
    }

    /// @brief Instance target function with derivatives used by minpack.
    ///        Calls either the user defined target function f(x) or computes
    ///        the Jacobian matrix J(x) for the current parameters x.
    inline int MinpackFunction_(int m, int n, const double *x, double *fvec,
                                double *fjac, int ldfjac, int iflag)
    {
      int res = -1;
      memcpy(LM_parameters_.GetData(), x, n * sizeof(double));
      if (iflag == 1)
      {
        // Compute target function f(x) and store results internally
        res = LM_TargetFunction_(LM_parameters_, LM_residuals_);
        memcpy(fvec, LM_residuals_.GetData(), m * sizeof(double));
        LM_numIters_++; // count evaluations of target function
      }
      else if (iflag == 2)
      {
        // Compute Jacobian J(x) and store results internally
        res = LM_JacobianFunction_(LM_parameters_, LM_Jacobian_);
        // @todo Test leading dimension ldfjac here!
        double *f = fjac;
        for (int i = 0; i < n; i++)
          for (int j = 0; j < m; j++)
            *(f++) = LM_Jacobian_[j][i];
      }
      else
      {
        // @todo What about other values for iflag?
        res = iflag;
      }
      return res;
    }

    /// @brief Instance target function w/o derivatives used by minpack.
    ///        Simply calls the user defined target function f(x) for
    ///        the current parameters x.
    inline int MinpackFunctionWithoutJacobian_(int m, int n, const double *x,
                                               double *fvec, int iflag)
    {
      int res = -1;
      memcpy(LM_parameters_.GetData(), x, n * sizeof(double));
      if (iflag == 0 || iflag == 1)
      {
        // Compute target function f(x) and store results internally
        res = LM_TargetFunction_(LM_parameters_, LM_residuals_);
        memcpy(fvec, LM_residuals_.GetData(), m * sizeof(double));
        LM_numIters_++; // count evaluations of target function
      }
      else
      {
        // @todo What about other values for iflag?
        res = iflag;
      }
      return res;
    }

  };
}
#endif // _LevenbergMarquardtBase_hh_