LevenbergMarquardt.h
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
00005 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
00006 //
00007 // The algorithm of this class initially comes from MINPACK whose original authors are:
00008 // Copyright Jorge More - Argonne National Laboratory
00009 // Copyright Burt Garbow - Argonne National Laboratory
00010 // Copyright Ken Hillstrom - Argonne National Laboratory
00011 //
00012 // This Source Code Form is subject to the terms of the Minpack license
00013 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
00014 //
00015 // This Source Code Form is subject to the terms of the Mozilla
00016 // Public License v. 2.0. If a copy of the MPL was not distributed
00017 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
00018 
00019 #ifndef EIGEN_LEVENBERGMARQUARDT_H
00020 #define EIGEN_LEVENBERGMARQUARDT_H
00021 
00022 
00023 namespace Eigen {
00024 namespace LevenbergMarquardtSpace {
00025     enum Status {
00026         NotStarted = -2,
00027         Running = -1,
00028         ImproperInputParameters = 0,
00029         RelativeReductionTooSmall = 1,
00030         RelativeErrorTooSmall = 2,
00031         RelativeErrorAndReductionTooSmall = 3,
00032         CosinusTooSmall = 4,
00033         TooManyFunctionEvaluation = 5,
00034         FtolTooSmall = 6,
00035         XtolTooSmall = 7,
00036         GtolTooSmall = 8,
00037         UserAsked = 9
00038     };
00039 }
00040 
00041 template <typename _Scalar, int NX=Dynamic, int NY=Dynamic>
00042 struct DenseFunctor
00043 {
00044   typedef _Scalar Scalar;
00045   enum {
00046     InputsAtCompileTime = NX,
00047     ValuesAtCompileTime = NY
00048   };
00049   typedef Matrix<Scalar,InputsAtCompileTime,1> InputType;
00050   typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
00051   typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
00052   typedef ColPivHouseholderQR<JacobianType> QRSolver;
00053   const int m_inputs, m_values;
00054 
00055   DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
00056   DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
00057 
00058   int inputs() const { return m_inputs; }
00059   int values() const { return m_values; }
00060 
00061   //int operator()(const InputType &x, ValueType& fvec) { }
00062   // should be defined in derived classes
00063   
00064   //int df(const InputType &x, JacobianType& fjac) { }
00065   // should be defined in derived classes
00066 };
00067 
00068 template <typename _Scalar, typename _Index>
00069 struct SparseFunctor
00070 {
00071   typedef _Scalar Scalar;
00072   typedef _Index Index;
00073   typedef Matrix<Scalar,Dynamic,1> InputType;
00074   typedef Matrix<Scalar,Dynamic,1> ValueType;
00075   typedef SparseMatrix<Scalar, ColMajor, Index> JacobianType;
00076   typedef SparseQR<JacobianType, COLAMDOrdering<int> > QRSolver;
00077   enum {
00078     InputsAtCompileTime = Dynamic,
00079     ValuesAtCompileTime = Dynamic
00080   };
00081   
00082   SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
00083 
00084   int inputs() const { return m_inputs; }
00085   int values() const { return m_values; }
00086   
00087   const int m_inputs, m_values;
00088   //int operator()(const InputType &x, ValueType& fvec) { }
00089   // to be defined in the functor
00090   
00091   //int df(const InputType &x, JacobianType& fjac) { }
00092   // to be defined in the functor if no automatic differentiation
00093   
00094 };
00095 namespace internal {
00096 template <typename QRSolver, typename VectorType>
00097 void lmpar2(const QRSolver &qr, const VectorType  &diag, const VectorType  &qtb,
00098             typename VectorType::Scalar m_delta, typename VectorType::Scalar &par,
00099             VectorType  &x);
00100     }
00109 template<typename _FunctorType>
00110 class LevenbergMarquardt : internal::no_assignment_operator
00111 {
00112   public:
00113     typedef _FunctorType FunctorType;
00114     typedef typename FunctorType::QRSolver QRSolver;
00115     typedef typename FunctorType::JacobianType JacobianType;
00116     typedef typename JacobianType::Scalar Scalar;
00117     typedef typename JacobianType::RealScalar RealScalar; 
00118     typedef typename QRSolver::StorageIndex PermIndex;
00119     typedef Matrix<Scalar,Dynamic,1> FVectorType;
00120     typedef PermutationMatrix<Dynamic,Dynamic> PermutationType;
00121   public:
00122     LevenbergMarquardt(FunctorType& functor) 
00123     : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0),
00124       m_isInitialized(false),m_info(InvalidInput)
00125     {
00126       resetParameters();
00127       m_useExternalScaling=false; 
00128     }
00129     
00130     LevenbergMarquardtSpace::Status minimize(FVectorType &x);
00131     LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
00132     LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
00133     LevenbergMarquardtSpace::Status lmder1(
00134       FVectorType  &x, 
00135       const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
00136     );
00137     static LevenbergMarquardtSpace::Status lmdif1(
00138             FunctorType &functor,
00139             FVectorType  &x,
00140             Index *nfev,
00141             const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
00142             );
00143     
00145     void resetParameters() 
00146     {
00147       using std::sqrt;        
00148 
00149       m_factor = 100.; 
00150       m_maxfev = 400; 
00151       m_ftol = sqrt(NumTraits<RealScalar>::epsilon());
00152       m_xtol = sqrt(NumTraits<RealScalar>::epsilon());
00153       m_gtol = 0. ; 
00154       m_epsfcn = 0. ;
00155     }
00156     
00158     void setXtol(RealScalar xtol) { m_xtol = xtol; }
00159     
00161     void setFtol(RealScalar ftol) { m_ftol = ftol; }
00162     
00164     void setGtol(RealScalar gtol) { m_gtol = gtol; }
00165     
00167     void setFactor(RealScalar factor) { m_factor = factor; }    
00168     
00170     void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; }
00171     
00173     void setMaxfev(Index maxfev) {m_maxfev = maxfev; }
00174     
00176     void setExternalScaling(bool value) {m_useExternalScaling  = value; }
00177     
00179     RealScalar xtol() const {return m_xtol; }
00180     
00182     RealScalar ftol() const {return m_ftol; }
00183     
00185     RealScalar gtol() const {return m_gtol; }
00186     
00188     RealScalar factor() const {return m_factor; }
00189     
00191     RealScalar epsilon() const {return m_epsfcn; }
00192     
00194     Index maxfev() const {return m_maxfev; }
00195     
00197     FVectorType& diag() {return m_diag; }
00198     
00200     Index iterations() { return m_iter; }
00201     
00203     Index nfev() { return m_nfev; }
00204     
00206     Index njev() { return m_njev; }
00207     
00209     RealScalar fnorm() {return m_fnorm; }
00210     
00212     RealScalar gnorm() {return m_gnorm; }
00213     
00215     RealScalar lm_param(void) { return m_par; }
00216     
00219     FVectorType& fvec() {return m_fvec; }
00220     
00223     JacobianType& jacobian() {return m_fjac; }
00224     
00228     JacobianType& matrixR() {return m_rfactor; }
00229     
00232     PermutationType permutation() {return m_permutation; }
00233     
00243     ComputationInfo info() const
00244     {
00245       
00246       return m_info;
00247     }
00248   private:
00249     JacobianType m_fjac; 
00250     JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac
00251     FunctorType &m_functor;
00252     FVectorType m_fvec, m_qtf, m_diag; 
00253     Index n;
00254     Index m; 
00255     Index m_nfev;
00256     Index m_njev; 
00257     RealScalar m_fnorm; // Norm of the current vector function
00258     RealScalar m_gnorm; //Norm of the gradient of the error 
00259     RealScalar m_factor; //
00260     Index m_maxfev; // Maximum number of function evaluation
00261     RealScalar m_ftol; //Tolerance in the norm of the vector function
00262     RealScalar m_xtol; // 
00263     RealScalar m_gtol; //tolerance of the norm of the error gradient
00264     RealScalar m_epsfcn; //
00265     Index m_iter; // Number of iterations performed
00266     RealScalar m_delta;
00267     bool m_useExternalScaling;
00268     PermutationType m_permutation;
00269     FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors
00270     RealScalar m_par;
00271     bool m_isInitialized; // Check whether the minimization step has been called
00272     ComputationInfo m_info; 
00273 };
00274 
00275 template<typename FunctorType>
00276 LevenbergMarquardtSpace::Status
00277 LevenbergMarquardt<FunctorType>::minimize(FVectorType  &x)
00278 {
00279     LevenbergMarquardtSpace::Status status = minimizeInit(x);
00280     if (status==LevenbergMarquardtSpace::ImproperInputParameters) {
00281       m_isInitialized = true;
00282       return status;
00283     }
00284     do {
00285 //       std::cout << " uv " << x.transpose() << "\n";
00286         status = minimizeOneStep(x);
00287     } while (status==LevenbergMarquardtSpace::Running);
00288      m_isInitialized = true;
00289      return status;
00290 }
00291 
00292 template<typename FunctorType>
00293 LevenbergMarquardtSpace::Status
00294 LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType  &x)
00295 {
00296     n = x.size();
00297     m = m_functor.values();
00298 
00299     m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n);
00300     m_wa4.resize(m);
00301     m_fvec.resize(m);
00302     //FIXME Sparse Case : Allocate space for the jacobian
00303     m_fjac.resize(m, n);
00304 //     m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative
00305     if (!m_useExternalScaling)
00306         m_diag.resize(n);
00307     eigen_assert( (!m_useExternalScaling || m_diag.size()==n) && "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'");
00308     m_qtf.resize(n);
00309 
00310     /* Function Body */
00311     m_nfev = 0;
00312     m_njev = 0;
00313 
00314     /*     check the input parameters for errors. */
00315     if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){
00316       m_info = InvalidInput;
00317       return LevenbergMarquardtSpace::ImproperInputParameters;
00318     }
00319 
00320     if (m_useExternalScaling)
00321         for (Index j = 0; j < n; ++j)
00322             if (m_diag[j] <= 0.) 
00323             {
00324               m_info = InvalidInput;
00325               return LevenbergMarquardtSpace::ImproperInputParameters;
00326             }
00327 
00328     /*     evaluate the function at the starting point */
00329     /*     and calculate its norm. */
00330     m_nfev = 1;
00331     if ( m_functor(x, m_fvec) < 0)
00332         return LevenbergMarquardtSpace::UserAsked;
00333     m_fnorm = m_fvec.stableNorm();
00334 
00335     /*     initialize levenberg-marquardt parameter and iteration counter. */
00336     m_par = 0.;
00337     m_iter = 1;
00338 
00339     return LevenbergMarquardtSpace::NotStarted;
00340 }
00341 
00342 template<typename FunctorType>
00343 LevenbergMarquardtSpace::Status
00344 LevenbergMarquardt<FunctorType>::lmder1(
00345         FVectorType  &x,
00346         const Scalar tol
00347         )
00348 {
00349     n = x.size();
00350     m = m_functor.values();
00351 
00352     /* check the input parameters for errors. */
00353     if (n <= 0 || m < n || tol < 0.)
00354         return LevenbergMarquardtSpace::ImproperInputParameters;
00355 
00356     resetParameters();
00357     m_ftol = tol;
00358     m_xtol = tol;
00359     m_maxfev = 100*(n+1);
00360 
00361     return minimize(x);
00362 }
00363 
00364 
00365 template<typename FunctorType>
00366 LevenbergMarquardtSpace::Status
00367 LevenbergMarquardt<FunctorType>::lmdif1(
00368         FunctorType &functor,
00369         FVectorType  &x,
00370         Index *nfev,
00371         const Scalar tol
00372         )
00373 {
00374     Index n = x.size();
00375     Index m = functor.values();
00376 
00377     /* check the input parameters for errors. */
00378     if (n <= 0 || m < n || tol < 0.)
00379         return LevenbergMarquardtSpace::ImproperInputParameters;
00380 
00381     NumericalDiff<FunctorType> numDiff(functor);
00382     // embedded LevenbergMarquardt
00383     LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff);
00384     lm.setFtol(tol);
00385     lm.setXtol(tol);
00386     lm.setMaxfev(200*(n+1));
00387 
00388     LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
00389     if (nfev)
00390         * nfev = lm.nfev();
00391     return info;
00392 }
00393 
00394 } // end namespace Eigen
00395 
00396 #endif // EIGEN_LEVENBERGMARQUARDT_H
 All Classes Functions Variables Typedefs Enumerator