src/LevenbergMarquardt/LevenbergMarquardt.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
0005 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
0006 //
0007 // The algorithm of this class initially comes from MINPACK whose original authors are:
0008 // Copyright Jorge More - Argonne National Laboratory
0009 // Copyright Burt Garbow - Argonne National Laboratory
0010 // Copyright Ken Hillstrom - Argonne National Laboratory
0011 //
0012 // This Source Code Form is subject to the terms of the Minpack license
0013 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
0014 //
0015 // This Source Code Form is subject to the terms of the Mozilla
0016 // Public License v. 2.0. If a copy of the MPL was not distributed
0017 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
0018
0019 #ifndef EIGEN_LEVENBERGMARQUARDT_H
0020 #define EIGEN_LEVENBERGMARQUARDT_H
0021
0022
0023 namespace Eigen {
0024 namespace LevenbergMarquardtSpace {
0025     enum Status {
0026         NotStarted = -2,
0027         Running = -1,
0028         ImproperInputParameters = 0,
0029         RelativeReductionTooSmall = 1,
0030         RelativeErrorTooSmall = 2,
0031         RelativeErrorAndReductionTooSmall = 3,
0032         CosinusTooSmall = 4,
0033         TooManyFunctionEvaluation = 5,
0034         FtolTooSmall = 6,
0035         XtolTooSmall = 7,
0036         GtolTooSmall = 8,
0037         UserAsked = 9
0038     };
0039 }
0040
0041 template <typename _Scalar, int NX=Dynamic, int NY=Dynamic>
0042 struct DenseFunctor
0043 {
0044   typedef _Scalar Scalar;
0045   enum {
0046     InputsAtCompileTime = NX,
0047     ValuesAtCompileTime = NY
0048   };
0049   typedef Matrix<Scalar,InputsAtCompileTime,1> InputType;
0050   typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
0051   typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
0052   typedef ColPivHouseholderQR<JacobianType> QRSolver;
0053   const int m_inputs, m_values;
0054
0055   DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
0056   DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
0057
0058   int inputs() const { return m_inputs; }
0059   int values() const { return m_values; }
0060
0061   //int operator()(const InputType &x, ValueType& fvec) { }
0062   // should be defined in derived classes
0063
0064   //int df(const InputType &x, JacobianType& fjac) { }
0065   // should be defined in derived classes
0066 };
0067
0068 template <typename _Scalar, typename _Index>
0069 struct SparseFunctor
0070 {
0071   typedef _Scalar Scalar;
0072   typedef _Index Index;
0073   typedef Matrix<Scalar,Dynamic,1> InputType;
0074   typedef Matrix<Scalar,Dynamic,1> ValueType;
0075   typedef SparseMatrix<Scalar, ColMajor, Index> JacobianType;
0076   typedef SparseQR<JacobianType, COLAMDOrdering<int> > QRSolver;
0077   enum {
0078     InputsAtCompileTime = Dynamic,
0079     ValuesAtCompileTime = Dynamic
0080   };
0081
0082   SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
0083
0084   int inputs() const { return m_inputs; }
0085   int values() const { return m_values; }
0086
0087   const int m_inputs, m_values;
0088   //int operator()(const InputType &x, ValueType& fvec) { }
0089   // to be defined in the functor
0090
0091   //int df(const InputType &x, JacobianType& fjac) { }
0092   // to be defined in the functor if no automatic differentiation
0093
0094 };
0095 namespace internal {
0096 template <typename QRSolver, typename VectorType>
0097 void lmpar2(const QRSolver &qr, const VectorType  &diag, const VectorType  &qtb,
0098         typename VectorType::Scalar m_delta, typename VectorType::Scalar &par,
0099         VectorType  &x);
0100     }
0101 /**
0102   * \ingroup NonLinearOptimization_Module
0103   * \brief Performs non linear optimization over a non-linear function,
0104   * using a variant of the Levenberg Marquardt algorithm.
0105   *
0106   * Check wikipedia for more information.
0107   * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm
0108   */
0109 template<typename _FunctorType>
0110 class LevenbergMarquardt : internal::no_assignment_operator
0111 {
0112   public:
0113     typedef _FunctorType FunctorType;
0114     typedef typename FunctorType::QRSolver QRSolver;
0115     typedef typename FunctorType::JacobianType JacobianType;
0116     typedef typename JacobianType::Scalar Scalar;
0117     typedef typename JacobianType::RealScalar RealScalar;
0118     typedef typename QRSolver::StorageIndex PermIndex;
0119     typedef Matrix<Scalar,Dynamic,1> FVectorType;
0120     typedef PermutationMatrix<Dynamic,Dynamic,int> PermutationType;
0121   public:
0122     LevenbergMarquardt(FunctorType& functor)
0123     : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0),
0124       m_isInitialized(false),m_info(InvalidInput)
0125     {
0126       resetParameters();
0127       m_useExternalScaling=false;
0128     }
0129
0130     LevenbergMarquardtSpace::Status minimize(FVectorType &x);
0131     LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
0132     LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
0133     LevenbergMarquardtSpace::Status lmder1(
0134       FVectorType  &x,
0135       const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
0136     );
0137     static LevenbergMarquardtSpace::Status lmdif1(
0138             FunctorType &functor,
0139             FVectorType  &x,
0140             Index *nfev,
0141             const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
0142             );
0143
0144     /** Sets the default parameters */
0145     void resetParameters()
0146     {
0147       using std::sqrt;
0148
0149       m_factor = 100.;
0150       m_maxfev = 400;
0151       m_ftol = sqrt(NumTraits<RealScalar>::epsilon());
0152       m_xtol = sqrt(NumTraits<RealScalar>::epsilon());
0153       m_gtol = 0. ;
0154       m_epsfcn = 0. ;
0155     }
0156
0157     /** Sets the tolerance for the norm of the solution vector*/
0158     void setXtol(RealScalar xtol) { m_xtol = xtol; }
0159
0160     /** Sets the tolerance for the norm of the vector function*/
0161     void setFtol(RealScalar ftol) { m_ftol = ftol; }
0162
0163     /** Sets the tolerance for the norm of the gradient of the error vector*/
0164     void setGtol(RealScalar gtol) { m_gtol = gtol; }
0165
0166     /** Sets the step bound for the diagonal shift */
0167     void setFactor(RealScalar factor) { m_factor = factor; }
0168
0169     /** Sets the error precision  */
0170     void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; }
0171
0172     /** Sets the maximum number of function evaluation */
0173     void setMaxfev(Index maxfev) {m_maxfev = maxfev; }
0174
0175     /** Use an external Scaling. If set to true, pass a nonzero diagonal to diag() */
0176     void setExternalScaling(bool value) {m_useExternalScaling  = value; }
0177
0178     /** \returns the tolerance for the norm of the solution vector */
0179     RealScalar xtol() const {return m_xtol; }
0180
0181     /** \returns the tolerance for the norm of the vector function */
0182     RealScalar ftol() const {return m_ftol; }
0183
0184     /** \returns the tolerance for the norm of the gradient of the error vector */
0185     RealScalar gtol() const {return m_gtol; }
0186
0187     /** \returns the step bound for the diagonal shift */
0188     RealScalar factor() const {return m_factor; }
0189
0190     /** \returns the error precision */
0191     RealScalar epsilon() const {return m_epsfcn; }
0192
0193     /** \returns the maximum number of function evaluation */
0194     Index maxfev() const {return m_maxfev; }
0195
0196     /** \returns a reference to the diagonal of the jacobian */
0197     FVectorType& diag() {return m_diag; }
0198
0199     /** \returns the number of iterations performed */
0200     Index iterations() { return m_iter; }
0201
0202     /** \returns the number of functions evaluation */
0203     Index nfev() { return m_nfev; }
0204
0205     /** \returns the number of jacobian evaluation */
0206     Index njev() { return m_njev; }
0207
0208     /** \returns the norm of current vector function */
0209     RealScalar fnorm() {return m_fnorm; }
0210
0211     /** \returns the norm of the gradient of the error */
0212     RealScalar gnorm() {return m_gnorm; }
0213
0214     /** \returns the LevenbergMarquardt parameter */
0215     RealScalar lm_param(void) { return m_par; }
0216
0217     /** \returns a reference to the  current vector function
0218      */
0219     FVectorType& fvec() {return m_fvec; }
0220
0221     /** \returns a reference to the matrix where the current Jacobian matrix is stored
0222      */
0223     JacobianType& jacobian() {return m_fjac; }
0224
0225     /** \returns a reference to the triangular matrix R from the QR of the jacobian matrix.
0226      * \sa jacobian()
0227      */
0228     JacobianType& matrixR() {return m_rfactor; }
0229
0230     /** the permutation used in the QR factorization
0231      */
0232     PermutationType permutation() {return m_permutation; }
0233
0234     /**
0235      * \brief Reports whether the minimization was successful
0236      * \returns \c Success if the minimization was successful,
0237      *         \c NumericalIssue if a numerical problem arises during the
0238      *          minimization process, for example during the QR factorization
0239      *         \c NoConvergence if the minimization did not converge after
0240      *          the maximum number of function evaluation allowed
0241      *          \c InvalidInput if the input matrix is invalid
0242      */
0243     ComputationInfo info() const
0244     {
0245
0246       return m_info;
0247     }
0248   private:
0249     JacobianType m_fjac;
0250     JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac
0251     FunctorType &m_functor;
0252     FVectorType m_fvec, m_qtf, m_diag;
0253     Index n;
0254     Index m;
0255     Index m_nfev;
0256     Index m_njev;
0257     RealScalar m_fnorm; // Norm of the current vector function
0258     RealScalar m_gnorm; //Norm of the gradient of the error
0259     RealScalar m_factor; //
0260     Index m_maxfev; // Maximum number of function evaluation
0261     RealScalar m_ftol; //Tolerance in the norm of the vector function
0262     RealScalar m_xtol; //
0263     RealScalar m_gtol; //tolerance of the norm of the error gradient
0264     RealScalar m_epsfcn; //
0265     Index m_iter; // Number of iterations performed
0266     RealScalar m_delta;
0267     bool m_useExternalScaling;
0268     PermutationType m_permutation;
0269     FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors
0270     RealScalar m_par;
0271     bool m_isInitialized; // Check whether the minimization step has been called
0272     ComputationInfo m_info;
0273 };
0274
0275 template<typename FunctorType>
0276 LevenbergMarquardtSpace::Status
0277 LevenbergMarquardt<FunctorType>::minimize(FVectorType  &x)
0278 {
0279     LevenbergMarquardtSpace::Status status = minimizeInit(x);
0280     if (status==LevenbergMarquardtSpace::ImproperInputParameters) {
0281       m_isInitialized = true;
0282       return status;
0283     }
0284     do {
0285 //       std::cout << " uv " << x.transpose() << "\n";
0286         status = minimizeOneStep(x);
0287     } while (status==LevenbergMarquardtSpace::Running);
0288      m_isInitialized = true;
0289      return status;
0290 }
0291
0292 template<typename FunctorType>
0293 LevenbergMarquardtSpace::Status
0294 LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType  &x)
0295 {
0296     n = x.size();
0297     m = m_functor.values();
0298
0299     m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n);
0300     m_wa4.resize(m);
0301     m_fvec.resize(m);
0302     //FIXME Sparse Case : Allocate space for the jacobian
0303     m_fjac.resize(m, n);
0304 //     m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative
0305     if (!m_useExternalScaling)
0306         m_diag.resize(n);
0307     eigen_assert( (!m_useExternalScaling || m_diag.size()==n) && "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'");
0308     m_qtf.resize(n);
0309
0310     /* Function Body */
0311     m_nfev = 0;
0312     m_njev = 0;
0313
0314     /*     check the input parameters for errors. */
0315     if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){
0316       m_info = InvalidInput;
0317       return LevenbergMarquardtSpace::ImproperInputParameters;
0318     }
0319
0320     if (m_useExternalScaling)
0321         for (Index j = 0; j < n; ++j)
0322             if (m_diag[j] <= 0.)
0323             {
0324               m_info = InvalidInput;
0325               return LevenbergMarquardtSpace::ImproperInputParameters;
0326             }
0327
0328     /*     evaluate the function at the starting point */
0329     /*     and calculate its norm. */
0330     m_nfev = 1;
0331     if ( m_functor(x, m_fvec) < 0)
0332         return LevenbergMarquardtSpace::UserAsked;
0333     m_fnorm = m_fvec.stableNorm();
0334
0335     /*     initialize levenberg-marquardt parameter and iteration counter. */
0336     m_par = 0.;
0337     m_iter = 1;
0338
0339     return LevenbergMarquardtSpace::NotStarted;
0340 }
0341
0342 template<typename FunctorType>
0343 LevenbergMarquardtSpace::Status
0344 LevenbergMarquardt<FunctorType>::lmder1(
0345         FVectorType  &x,
0346         const Scalar tol
0347         )
0348 {
0349     n = x.size();
0350     m = m_functor.values();
0351
0352     /* check the input parameters for errors. */
0353     if (n <= 0 || m < n || tol < 0.)
0354         return LevenbergMarquardtSpace::ImproperInputParameters;
0355
0356     resetParameters();
0357     m_ftol = tol;
0358     m_xtol = tol;
0359     m_maxfev = 100*(n+1);
0360
0361     return minimize(x);
0362 }
0363
0364
0365 template<typename FunctorType>
0366 LevenbergMarquardtSpace::Status
0367 LevenbergMarquardt<FunctorType>::lmdif1(
0368         FunctorType &functor,
0369         FVectorType  &x,
0370         Index *nfev,
0371         const Scalar tol
0372         )
0373 {
0374     Index n = x.size();
0375     Index m = functor.values();
0376
0377     /* check the input parameters for errors. */
0378     if (n <= 0 || m < n || tol < 0.)
0379         return LevenbergMarquardtSpace::ImproperInputParameters;
0380
0381     NumericalDiff<FunctorType> numDiff(functor);
0382     // embedded LevenbergMarquardt
0383     LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff);
0384     lm.setFtol(tol);
0385     lm.setXtol(tol);
0386     lm.setMaxfev(200*(n+1));
0387
0388     LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
0389     if (nfev)
0390         * nfev = lm.nfev();
0391     return info;
0392 }
0393
0394 } // end namespace Eigen
0395
0396 #endif // EIGEN_LEVENBERGMARQUARDT_H