ExampleLevenbergMarquardt.cpp

Describes implementation of non-linear optimization using the Levenberg-Marquardt algorithm by deriving from the base class LevenbergMarquardtBase instead of using the interface from Minpack directly. , Minpack

Author

esquivel 12/2011

/* 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
*/

/** @example ExampleLevenbergMarquardt.cpp
    @relates LevenbergMarquardtBase, Minpack
    @ingroup g_examples    
    @brief   Describes implementation of non-linear optimization using the
             Levenberg-Marquardt algorithm by deriving from the base class
             LevenbergMarquardtBase instead of using the interface from
             Minpack directly.
    @author  esquivel 12/2011
*/

#include <bias_config.h>
#include <Base/Debug/Debug.hh>
#include <Base/Debug/TimeMeasure.hh>
#include <Base/Math/Vector2.hh>
#include <MathAlgo/LevenbergMarquardtBase.hh>
#include <math.h>

using namespace BIAS;
using namespace std;

namespace BIAS
{

  /**
     @class ExampleLevenbergMarquardt
     @brief Example implementation of Levenberg-Marquardt algorithm to solve
            the equation y = a*cos(b*x) + b*sin(a*x) with parameters (a,b).

     The target function f(a,b) returns a vector of size m for m measurements
     (x_i,y_i) and has two parameters (a,b). f(a,b) defined elementwise as
     
       f_i(a,b) = a*cos(b*x_i) + b*sin(a*x_i) - y_i
     
     f(a,b) is defined by overwriting LevenbergMarquardt::LM_TargetFunction_.

     The Jacobian J(a,b) of the target function is a matrix of size m x n for
     n parameters, hence here it is of size m x 2. The Jacobian matrix J(a,b)
     is computed analytically with the i-th row of J(a,b) given by

        J_i(a,b) = (df_i/da, df_i/db) evaluated at (a,b)

        = ( cos(b*x_i) + x_i*b*cos(a*x_i) , -x_i*a*sin(b*x_i) + sin(a*x_i) )

     J(a,b) is defined by overwriting LevenbergMarquardt::LM_JacobianFunction_.
  */
  class ExampleLevenbergMarquardt : public BIAS::LevenbergMarquardtBase, BIAS::Debug
  {

  public:

    ExampleLevenbergMarquardt()
    {
      NewDebugLevel("ExampleLM_EVAL_FUN"); // show target function evaluations
      NewDebugLevel("ExampleLM_EVAL_JAC"); // show evaluations of its Jacobian
      //AddDebugLevel("ExampleLM_EVAL_FUN");
      //AddDebugLevel("ExampleLM_EVAL_JAC");
    }

    ~ExampleLevenbergMarquardt() {}

    int Compute(const std::vector< BIAS::Vector2<double> > &data,
                const double a0, const double b0, double &a, double &b,
                bool approximateJacobian = false)
    {
      // Prepare input data and target function
      data_ = data;
      numData_ = data.size();
      BIAS::Vector<double> paramsStart(2), paramsResult(2), fResult(numData_);
      paramsStart[0] = a0;
      paramsStart[1] = b0;

      cout << "- Levenberg-Marquardt algorithm starts at (a0,b0) = "
           << paramsStart << endl;

      // Call Levenberg-Marquardt algorithm
      int res = -1;
      if (!approximateJacobian) {
        res = LM_Compute(numData_, 2, paramsStart, paramsResult, fResult);
      } else {
        res = LM_ComputeWithoutJacobian(numData_, 2, paramsStart,
                                        paramsResult, fResult);
      }

      // Evaluate results
      if (res == 0)
      {
        a = paramsResult[0];
        b = paramsResult[1];
        cout << "- Levenberg-Marquardt algorithm finished successfully." << endl;
        cout << "- Solution is (a,b) = " << paramsResult
             << " with final target function value f(a,b) = " << fResult
             << " and sum of squared residuals |f(a,b)|^2 = "
             << LM_GetSumOfSquaredErrors() << " after " << LM_GetNumOfIterations()
             << " iterations (max. " << LM_GetMaxIterations() << ")" << endl << endl;
      } else {
        cout << "- Levenberg-Marquardt algorithm failed (return code " << res
             << ")!" << endl << endl;
      }
      return res;
    }

  protected:
    
    int numData_;
    std::vector< BIAS::Vector2<double> > data_;

    /// @brief Compute residuals of target function f(a,b) for parameters (a,b)
    virtual int LM_TargetFunction_(const BIAS::Vector<double> &params,
                                   BIAS::Vector<double> &residuals)
    {
      double a = params[0];
      double b = params[1];
      double x, y;
      for (int i = 0; i < numData_; i++) {
        x = data_[i][0];
        y = data_[i][1];
        residuals[i] = a*cos(b*x) + b*sin(a*x) - y;
      }
      if (DebugLevelIsSet("ExampleLM_EVAL_FUN"))
        cout << "--- " << LM_GetNumOfIterations() << " ----------------------------"
             << "----------------------------------" << endl << endl
             << "  (a,b) = " << params << ", f(a,b) = " << residuals
             << "  |f(a,b)| = " << residuals.NormL2() << endl << endl << flush;
      return 0;
    }

    /// @brief Compute Jacobian J(a,b) of target function f evaluated at (a,b)
    virtual int LM_JacobianFunction_(const BIAS::Vector<double> &params,
                                     BIAS::Matrix<double> &J)
    {
      double a = params[0];
      double b = params[1];
      double x;
      for (int i = 0; i < numData_; i++) {
        x = data_[i][0];
        J[i][0] =  cos(b*x) + x*b*cos(a*x);
        J[i][1] = -x*a*sin(b*x) + sin(a*x);
      }
      if (DebugLevelIsSet("ExampleLM_EVAL_JAC"))
        cout << "  (a,b) = " << params << ", J(a,b) = " << J << endl << flush;
      return 0;
    }
  };
}

int main()
{
  // Initialize Levenberg-Marquardt algorithm
  BIAS::ExampleLevenbergMarquardt LM;
  LM.LM_SetTolerance(1e-12);
  LM.LM_SetMaxIterations(1000);
  
  // Set ground truth parameters (a*,b*)
  double aGT = 100, bGT = 100;

  // Compute measurements f(a*,b*) to fit f(a,b) to
  const int numData = 8;
  std::vector< BIAS::Vector2<double> > data(numData);
  double x, y;
  for (int i = 0; i < numData; i++) {
    x = i / (double) numData * 1.8 * M_PI / bGT;
    y = aGT*cos(bGT*x) + bGT*sin(aGT*x);
    data[i].Set(x, y);
  }

  cout << endl << "--------------------------" << endl
       << " ExampleLevenbergMarquardt" << endl
       << "--------------------------" << endl
       << "- Compute parameters (a,b) minimizing the sum of squared residuals" << endl
       << "  of a*cos(b*x) + b*sin(a*x) - y from " << numData << " measurements (x,y)"
       << endl << endl << "- Tolerance is " << LM.LM_GetTolerance() << endl
       << "- Max. number of iterations is " << LM.LM_GetMaxIterations() << endl << endl;

  cout << "Computation with analytically given Jacobian" << endl
       << "----------------------------------------------" << endl << endl;

  // Minimize f(a,b) starting with point (a0,b0)
  double a0 = 66, b0 = 110;
  double aRes, bRes;
  int res = LM.Compute(data, a0, b0, aRes, bRes);
  if (res == 0) {
    cout << "-> final Jacobian J(a,b) is " << LM.LM_GetJacobianResult() << endl;
  }

  // Compare result (a,b) with ground truth parameters (a*,b*)
  if (res == 0) {
    cout << "-> estimation error is for parameter a = " << fabs(aRes-aGT)
         << " (" << 100 * (aRes != aGT ? fabs(aRes-aGT)/aGT : 0) << " %)"
         << endl << "   and for parameter b = " << fabs(bRes-bGT)
         << " (" << 100 * (bRes != bGT ? fabs(bRes-bGT)/bGT : 0) << " %)"
         << endl << endl;
  }

  cout << "Computation with numerically approximated Jacobian" << endl
       << "----------------------------------------------------" << endl << endl;

  // Now estimate the result with a numerical approximation of the Jacobian
  // and compare the results to the analytic solution
  res = LM.Compute(data, a0, b0, aRes, bRes, true);
  if (res == 0) {
    cout << "-> numerical approximation of final Jacobian J(a,b) is "
         << LM.LM_GetJacobianResult() << endl;
  }

  // Compare result (a,b) with ground truth parameters (a*,b*)
  if (res == 0) {
    cout << "-> estimation error with approximated Jacobian"
         << " is for parameter a = " << fabs(aRes-aGT)
         << " (" << 100 * (aRes != aGT ? fabs(aRes-aGT)/aGT : 0) << " %)"
         << endl << "   and for parameter b = " << fabs(bRes-bGT)
         << " (" << 100 * (bRes != bGT ? fabs(bRes-bGT)/bGT : 0) << " %)"
         << endl << endl;
  }

  return (res != 0 ? -1 : 0);
}