Home | History | Annotate | Download | only in LevenbergMarquardt 
      1 // This file is part of Eigen, a lightweight C++ template library
      2 // for linear algebra.
      3 //
      4 // Copyright (C) 2009 Thomas Capricelli <orzel (at) freehackers.org>
      5 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam (at) inria.fr>
      6 //
      7 // The algorithm of this class initially comes from MINPACK whose original authors are:
      8 // Copyright Jorge More - Argonne National Laboratory
      9 // Copyright Burt Garbow - Argonne National Laboratory
     10 // Copyright Ken Hillstrom - Argonne National Laboratory
     11 //
     12 // This Source Code Form is subject to the terms of the Minpack license
     13 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
     14 //
     15 // This Source Code Form is subject to the terms of the Mozilla
     16 // Public License v. 2.0. If a copy of the MPL was not distributed
     17 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
     18 
     19 #ifndef EIGEN_LEVENBERGMARQUARDT_H
     20 #define EIGEN_LEVENBERGMARQUARDT_H
     21 
     22 
     23 namespace Eigen {
     24 namespace LevenbergMarquardtSpace {
     25     enum Status {
     26         NotStarted = -2,
     27         Running = -1,
     28         ImproperInputParameters = 0,
     29         RelativeReductionTooSmall = 1,
     30         RelativeErrorTooSmall = 2,
     31         RelativeErrorAndReductionTooSmall = 3,
     32         CosinusTooSmall = 4,
     33         TooManyFunctionEvaluation = 5,
     34         FtolTooSmall = 6,
     35         XtolTooSmall = 7,
     36         GtolTooSmall = 8,
     37         UserAsked = 9
     38     };
     39 }
     40 
     41 template <typename _Scalar, int NX=Dynamic, int NY=Dynamic>
     42 struct DenseFunctor
     43 {
     44   typedef _Scalar Scalar;
     45   enum {
     46     InputsAtCompileTime = NX,
     47     ValuesAtCompileTime = NY
     48   };
     49   typedef Matrix<Scalar,InputsAtCompileTime,1> InputType;
     50   typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
     51   typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
     52   typedef ColPivHouseholderQR<JacobianType> QRSolver;
     53   const int m_inputs, m_values;
     54 
     55   DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
     56   DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
     57 
     58   int inputs() const { return m_inputs; }
     59   int values() const { return m_values; }
     60 
     61   //int operator()(const InputType &x, ValueType& fvec) { }
     62   // should be defined in derived classes
     63 
     64   //int df(const InputType &x, JacobianType& fjac) { }
     65   // should be defined in derived classes
     66 };
     67 
     68 template <typename _Scalar, typename _Index>
     69 struct SparseFunctor
     70 {
     71   typedef _Scalar Scalar;
     72   typedef _Index Index;
     73   typedef Matrix<Scalar,Dynamic,1> InputType;
     74   typedef Matrix<Scalar,Dynamic,1> ValueType;
     75   typedef SparseMatrix<Scalar, ColMajor, Index> JacobianType;
     76   typedef SparseQR<JacobianType, COLAMDOrdering<int> > QRSolver;
     77   enum {
     78     InputsAtCompileTime = Dynamic,
     79     ValuesAtCompileTime = Dynamic
     80   };
     81 
     82   SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
     83 
     84   int inputs() const { return m_inputs; }
     85   int values() const { return m_values; }
     86 
     87   const int m_inputs, m_values;
     88   //int operator()(const InputType &x, ValueType& fvec) { }
     89   // to be defined in the functor
     90 
     91   //int df(const InputType &x, JacobianType& fjac) { }
     92   // to be defined in the functor if no automatic differentiation
     93 
     94 };
     95 namespace internal {
     96 template <typename QRSolver, typename VectorType>
     97 void lmpar2(const QRSolver &qr, const VectorType  &diag, const VectorType  &qtb,
     98 	    typename VectorType::Scalar m_delta, typename VectorType::Scalar &par,
     99 	    VectorType  &x);
    100     }
    101 /**
    102   * \ingroup NonLinearOptimization_Module
    103   * \brief Performs non linear optimization over a non-linear function,
    104   * using a variant of the Levenberg Marquardt algorithm.
    105   *
    106   * Check wikipedia for more information.
    107   * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
    108   */
    109 template<typename _FunctorType>
    110 class LevenbergMarquardt : internal::no_assignment_operator
    111 {
    112   public:
    113     typedef _FunctorType FunctorType;
    114     typedef typename FunctorType::QRSolver QRSolver;
    115     typedef typename FunctorType::JacobianType JacobianType;
    116     typedef typename JacobianType::Scalar Scalar;
    117     typedef typename JacobianType::RealScalar RealScalar;
    118     typedef typename JacobianType::Index Index;
    119     typedef typename QRSolver::Index PermIndex;
    120     typedef Matrix<Scalar,Dynamic,1> FVectorType;
    121     typedef PermutationMatrix<Dynamic,Dynamic> PermutationType;
    122   public:
    123     LevenbergMarquardt(FunctorType& functor)
    124     : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0),
    125       m_isInitialized(false),m_info(InvalidInput)
    126     {
    127       resetParameters();
    128       m_useExternalScaling=false;
    129     }
    130 
    131     LevenbergMarquardtSpace::Status minimize(FVectorType &x);
    132     LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
    133     LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
    134     LevenbergMarquardtSpace::Status lmder1(
    135       FVectorType  &x,
    136       const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
    137     );
    138     static LevenbergMarquardtSpace::Status lmdif1(
    139             FunctorType &functor,
    140             FVectorType  &x,
    141             Index *nfev,
    142             const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
    143             );
    144 
    145     /** Sets the default parameters */
    146     void resetParameters()
    147     {
    148       m_factor = 100.;
    149       m_maxfev = 400;
    150       m_ftol = std::sqrt(NumTraits<RealScalar>::epsilon());
    151       m_xtol = std::sqrt(NumTraits<RealScalar>::epsilon());
    152       m_gtol = 0. ;
    153       m_epsfcn = 0. ;
    154     }
    155 
    156     /** Sets the tolerance for the norm of the solution vector*/
    157     void setXtol(RealScalar xtol) { m_xtol = xtol; }
    158 
    159     /** Sets the tolerance for the norm of the vector function*/
    160     void setFtol(RealScalar ftol) { m_ftol = ftol; }
    161 
    162     /** Sets the tolerance for the norm of the gradient of the error vector*/
    163     void setGtol(RealScalar gtol) { m_gtol = gtol; }
    164 
    165     /** Sets the step bound for the diagonal shift */
    166     void setFactor(RealScalar factor) { m_factor = factor; }
    167 
    168     /** Sets the error precision  */
    169     void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; }
    170 
    171     /** Sets the maximum number of function evaluation */
    172     void setMaxfev(Index maxfev) {m_maxfev = maxfev; }
    173 
    174     /** Use an external Scaling. If set to true, pass a nonzero diagonal to diag() */
    175     void setExternalScaling(bool value) {m_useExternalScaling  = value; }
    176 
    177     /** \returns a reference to the diagonal of the jacobian */
    178     FVectorType& diag() {return m_diag; }
    179 
    180     /** \returns the number of iterations performed */
    181     Index iterations() { return m_iter; }
    182 
    183     /** \returns the number of functions evaluation */
    184     Index nfev() { return m_nfev; }
    185 
    186     /** \returns the number of jacobian evaluation */
    187     Index njev() { return m_njev; }
    188 
    189     /** \returns the norm of current vector function */
    190     RealScalar fnorm() {return m_fnorm; }
    191 
    192     /** \returns the norm of the gradient of the error */
    193     RealScalar gnorm() {return m_gnorm; }
    194 
    195     /** \returns the LevenbergMarquardt parameter */
    196     RealScalar lm_param(void) { return m_par; }
    197 
    198     /** \returns a reference to the  current vector function
    199      */
    200     FVectorType& fvec() {return m_fvec; }
    201 
    202     /** \returns a reference to the matrix where the current Jacobian matrix is stored
    203      */
    204     JacobianType& jacobian() {return m_fjac; }
    205 
    206     /** \returns a reference to the triangular matrix R from the QR of the jacobian matrix.
    207      * \sa jacobian()
    208      */
    209     JacobianType& matrixR() {return m_rfactor; }
    210 
    211     /** the permutation used in the QR factorization
    212      */
    213     PermutationType permutation() {return m_permutation; }
    214 
    215     /**
    216      * \brief Reports whether the minimization was successful
    217      * \returns \c Success if the minimization was succesful,
    218      *         \c NumericalIssue if a numerical problem arises during the
    219      *          minimization process, for exemple during the QR factorization
    220      *         \c NoConvergence if the minimization did not converge after
    221      *          the maximum number of function evaluation allowed
    222      *          \c InvalidInput if the input matrix is invalid
    223      */
    224     ComputationInfo info() const
    225     {
    226 
    227       return m_info;
    228     }
    229   private:
    230     JacobianType m_fjac;
    231     JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac
    232     FunctorType &m_functor;
    233     FVectorType m_fvec, m_qtf, m_diag;
    234     Index n;
    235     Index m;
    236     Index m_nfev;
    237     Index m_njev;
    238     RealScalar m_fnorm; // Norm of the current vector function
    239     RealScalar m_gnorm; //Norm of the gradient of the error
    240     RealScalar m_factor; //
    241     Index m_maxfev; // Maximum number of function evaluation
    242     RealScalar m_ftol; //Tolerance in the norm of the vector function
    243     RealScalar m_xtol; //
    244     RealScalar m_gtol; //tolerance of the norm of the error gradient
    245     RealScalar m_epsfcn; //
    246     Index m_iter; // Number of iterations performed
    247     RealScalar m_delta;
    248     bool m_useExternalScaling;
    249     PermutationType m_permutation;
    250     FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors
    251     RealScalar m_par;
    252     bool m_isInitialized; // Check whether the minimization step has been called
    253     ComputationInfo m_info;
    254 };
    255 
    256 template<typename FunctorType>
    257 LevenbergMarquardtSpace::Status
    258 LevenbergMarquardt<FunctorType>::minimize(FVectorType  &x)
    259 {
    260     LevenbergMarquardtSpace::Status status = minimizeInit(x);
    261     if (status==LevenbergMarquardtSpace::ImproperInputParameters) {
    262       m_isInitialized = true;
    263       return status;
    264     }
    265     do {
    266 //       std::cout << " uv " << x.transpose() << "\n";
    267         status = minimizeOneStep(x);
    268     } while (status==LevenbergMarquardtSpace::Running);
    269      m_isInitialized = true;
    270      return status;
    271 }
    272 
    273 template<typename FunctorType>
    274 LevenbergMarquardtSpace::Status
    275 LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType  &x)
    276 {
    277     n = x.size();
    278     m = m_functor.values();
    279 
    280     m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n);
    281     m_wa4.resize(m);
    282     m_fvec.resize(m);
    283     //FIXME Sparse Case : Allocate space for the jacobian
    284     m_fjac.resize(m, n);
    285 //     m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative
    286     if (!m_useExternalScaling)
    287         m_diag.resize(n);
    288     eigen_assert( (!m_useExternalScaling || m_diag.size()==n) || "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'");
    289     m_qtf.resize(n);
    290 
    291     /* Function Body */
    292     m_nfev = 0;
    293     m_njev = 0;
    294 
    295     /*     check the input parameters for errors. */
    296     if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){
    297       m_info = InvalidInput;
    298       return LevenbergMarquardtSpace::ImproperInputParameters;
    299     }
    300 
    301     if (m_useExternalScaling)
    302         for (Index j = 0; j < n; ++j)
    303             if (m_diag[j] <= 0.)
    304             {
    305               m_info = InvalidInput;
    306               return LevenbergMarquardtSpace::ImproperInputParameters;
    307             }
    308 
    309     /*     evaluate the function at the starting point */
    310     /*     and calculate its norm. */
    311     m_nfev = 1;
    312     if ( m_functor(x, m_fvec) < 0)
    313         return LevenbergMarquardtSpace::UserAsked;
    314     m_fnorm = m_fvec.stableNorm();
    315 
    316     /*     initialize levenberg-marquardt parameter and iteration counter. */
    317     m_par = 0.;
    318     m_iter = 1;
    319 
    320     return LevenbergMarquardtSpace::NotStarted;
    321 }
    322 
    323 template<typename FunctorType>
    324 LevenbergMarquardtSpace::Status
    325 LevenbergMarquardt<FunctorType>::lmder1(
    326         FVectorType  &x,
    327         const Scalar tol
    328         )
    329 {
    330     n = x.size();
    331     m = m_functor.values();
    332 
    333     /* check the input parameters for errors. */
    334     if (n <= 0 || m < n || tol < 0.)
    335         return LevenbergMarquardtSpace::ImproperInputParameters;
    336 
    337     resetParameters();
    338     m_ftol = tol;
    339     m_xtol = tol;
    340     m_maxfev = 100*(n+1);
    341 
    342     return minimize(x);
    343 }
    344 
    345 
    346 template<typename FunctorType>
    347 LevenbergMarquardtSpace::Status
    348 LevenbergMarquardt<FunctorType>::lmdif1(
    349         FunctorType &functor,
    350         FVectorType  &x,
    351         Index *nfev,
    352         const Scalar tol
    353         )
    354 {
    355     Index n = x.size();
    356     Index m = functor.values();
    357 
    358     /* check the input parameters for errors. */
    359     if (n <= 0 || m < n || tol < 0.)
    360         return LevenbergMarquardtSpace::ImproperInputParameters;
    361 
    362     NumericalDiff<FunctorType> numDiff(functor);
    363     // embedded LevenbergMarquardt
    364     LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff);
    365     lm.setFtol(tol);
    366     lm.setXtol(tol);
    367     lm.setMaxfev(200*(n+1));
    368 
    369     LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
    370     if (nfev)
    371         * nfev = lm.nfev();
    372     return info;
    373 }
    374 
    375 } // end namespace Eigen
    376 
    377 #endif // EIGEN_LEVENBERGMARQUARDT_H
    378