• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
5 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@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 
DenseFunctorDenseFunctor55   DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
DenseFunctorDenseFunctor56   DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
57 
inputsDenseFunctor58   int inputs() const { return m_inputs; }
valuesDenseFunctor59   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 
SparseFunctorSparseFunctor82   SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
83 
inputsSparseFunctor84   int inputs() const { return m_inputs; }
valuesSparseFunctor85   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 QRSolver::StorageIndex PermIndex;
119     typedef Matrix<Scalar,Dynamic,1> FVectorType;
120     typedef PermutationMatrix<Dynamic,Dynamic> PermutationType;
121   public:
LevenbergMarquardt(FunctorType & functor)122     LevenbergMarquardt(FunctorType& functor)
123     : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0),
124       m_isInitialized(false),m_info(InvalidInput)
125     {
126       resetParameters();
127       m_useExternalScaling=false;
128     }
129 
130     LevenbergMarquardtSpace::Status minimize(FVectorType &x);
131     LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
132     LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
133     LevenbergMarquardtSpace::Status lmder1(
134       FVectorType  &x,
135       const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
136     );
137     static LevenbergMarquardtSpace::Status lmdif1(
138             FunctorType &functor,
139             FVectorType  &x,
140             Index *nfev,
141             const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
142             );
143 
144     /** Sets the default parameters */
resetParameters()145     void resetParameters()
146     {
147       using std::sqrt;
148 
149       m_factor = 100.;
150       m_maxfev = 400;
151       m_ftol = sqrt(NumTraits<RealScalar>::epsilon());
152       m_xtol = sqrt(NumTraits<RealScalar>::epsilon());
153       m_gtol = 0. ;
154       m_epsfcn = 0. ;
155     }
156 
157     /** Sets the tolerance for the norm of the solution vector*/
setXtol(RealScalar xtol)158     void setXtol(RealScalar xtol) { m_xtol = xtol; }
159 
160     /** Sets the tolerance for the norm of the vector function*/
setFtol(RealScalar ftol)161     void setFtol(RealScalar ftol) { m_ftol = ftol; }
162 
163     /** Sets the tolerance for the norm of the gradient of the error vector*/
setGtol(RealScalar gtol)164     void setGtol(RealScalar gtol) { m_gtol = gtol; }
165 
166     /** Sets the step bound for the diagonal shift */
setFactor(RealScalar factor)167     void setFactor(RealScalar factor) { m_factor = factor; }
168 
169     /** Sets the error precision  */
setEpsilon(RealScalar epsfcn)170     void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; }
171 
172     /** Sets the maximum number of function evaluation */
setMaxfev(Index maxfev)173     void setMaxfev(Index maxfev) {m_maxfev = maxfev; }
174 
175     /** Use an external Scaling. If set to true, pass a nonzero diagonal to diag() */
setExternalScaling(bool value)176     void setExternalScaling(bool value) {m_useExternalScaling  = value; }
177 
178     /** \returns the tolerance for the norm of the solution vector */
xtol()179     RealScalar xtol() const {return m_xtol; }
180 
181     /** \returns the tolerance for the norm of the vector function */
ftol()182     RealScalar ftol() const {return m_ftol; }
183 
184     /** \returns the tolerance for the norm of the gradient of the error vector */
gtol()185     RealScalar gtol() const {return m_gtol; }
186 
187     /** \returns the step bound for the diagonal shift */
factor()188     RealScalar factor() const {return m_factor; }
189 
190     /** \returns the error precision */
epsilon()191     RealScalar epsilon() const {return m_epsfcn; }
192 
193     /** \returns the maximum number of function evaluation */
maxfev()194     Index maxfev() const {return m_maxfev; }
195 
196     /** \returns a reference to the diagonal of the jacobian */
diag()197     FVectorType& diag() {return m_diag; }
198 
199     /** \returns the number of iterations performed */
iterations()200     Index iterations() { return m_iter; }
201 
202     /** \returns the number of functions evaluation */
nfev()203     Index nfev() { return m_nfev; }
204 
205     /** \returns the number of jacobian evaluation */
njev()206     Index njev() { return m_njev; }
207 
208     /** \returns the norm of current vector function */
fnorm()209     RealScalar fnorm() {return m_fnorm; }
210 
211     /** \returns the norm of the gradient of the error */
gnorm()212     RealScalar gnorm() {return m_gnorm; }
213 
214     /** \returns the LevenbergMarquardt parameter */
lm_param(void)215     RealScalar lm_param(void) { return m_par; }
216 
217     /** \returns a reference to the  current vector function
218      */
fvec()219     FVectorType& fvec() {return m_fvec; }
220 
221     /** \returns a reference to the matrix where the current Jacobian matrix is stored
222      */
jacobian()223     JacobianType& jacobian() {return m_fjac; }
224 
225     /** \returns a reference to the triangular matrix R from the QR of the jacobian matrix.
226      * \sa jacobian()
227      */
matrixR()228     JacobianType& matrixR() {return m_rfactor; }
229 
230     /** the permutation used in the QR factorization
231      */
permutation()232     PermutationType permutation() {return m_permutation; }
233 
234     /**
235      * \brief Reports whether the minimization was successful
236      * \returns \c Success if the minimization was succesful,
237      *         \c NumericalIssue if a numerical problem arises during the
238      *          minimization process, for exemple during the QR factorization
239      *         \c NoConvergence if the minimization did not converge after
240      *          the maximum number of function evaluation allowed
241      *          \c InvalidInput if the input matrix is invalid
242      */
info()243     ComputationInfo info() const
244     {
245 
246       return m_info;
247     }
248   private:
249     JacobianType m_fjac;
250     JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac
251     FunctorType &m_functor;
252     FVectorType m_fvec, m_qtf, m_diag;
253     Index n;
254     Index m;
255     Index m_nfev;
256     Index m_njev;
257     RealScalar m_fnorm; // Norm of the current vector function
258     RealScalar m_gnorm; //Norm of the gradient of the error
259     RealScalar m_factor; //
260     Index m_maxfev; // Maximum number of function evaluation
261     RealScalar m_ftol; //Tolerance in the norm of the vector function
262     RealScalar m_xtol; //
263     RealScalar m_gtol; //tolerance of the norm of the error gradient
264     RealScalar m_epsfcn; //
265     Index m_iter; // Number of iterations performed
266     RealScalar m_delta;
267     bool m_useExternalScaling;
268     PermutationType m_permutation;
269     FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors
270     RealScalar m_par;
271     bool m_isInitialized; // Check whether the minimization step has been called
272     ComputationInfo m_info;
273 };
274 
275 template<typename FunctorType>
276 LevenbergMarquardtSpace::Status
minimize(FVectorType & x)277 LevenbergMarquardt<FunctorType>::minimize(FVectorType  &x)
278 {
279     LevenbergMarquardtSpace::Status status = minimizeInit(x);
280     if (status==LevenbergMarquardtSpace::ImproperInputParameters) {
281       m_isInitialized = true;
282       return status;
283     }
284     do {
285 //       std::cout << " uv " << x.transpose() << "\n";
286         status = minimizeOneStep(x);
287     } while (status==LevenbergMarquardtSpace::Running);
288      m_isInitialized = true;
289      return status;
290 }
291 
292 template<typename FunctorType>
293 LevenbergMarquardtSpace::Status
minimizeInit(FVectorType & x)294 LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType  &x)
295 {
296     n = x.size();
297     m = m_functor.values();
298 
299     m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n);
300     m_wa4.resize(m);
301     m_fvec.resize(m);
302     //FIXME Sparse Case : Allocate space for the jacobian
303     m_fjac.resize(m, n);
304 //     m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative
305     if (!m_useExternalScaling)
306         m_diag.resize(n);
307     eigen_assert( (!m_useExternalScaling || m_diag.size()==n) && "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'");
308     m_qtf.resize(n);
309 
310     /* Function Body */
311     m_nfev = 0;
312     m_njev = 0;
313 
314     /*     check the input parameters for errors. */
315     if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){
316       m_info = InvalidInput;
317       return LevenbergMarquardtSpace::ImproperInputParameters;
318     }
319 
320     if (m_useExternalScaling)
321         for (Index j = 0; j < n; ++j)
322             if (m_diag[j] <= 0.)
323             {
324               m_info = InvalidInput;
325               return LevenbergMarquardtSpace::ImproperInputParameters;
326             }
327 
328     /*     evaluate the function at the starting point */
329     /*     and calculate its norm. */
330     m_nfev = 1;
331     if ( m_functor(x, m_fvec) < 0)
332         return LevenbergMarquardtSpace::UserAsked;
333     m_fnorm = m_fvec.stableNorm();
334 
335     /*     initialize levenberg-marquardt parameter and iteration counter. */
336     m_par = 0.;
337     m_iter = 1;
338 
339     return LevenbergMarquardtSpace::NotStarted;
340 }
341 
342 template<typename FunctorType>
343 LevenbergMarquardtSpace::Status
lmder1(FVectorType & x,const Scalar tol)344 LevenbergMarquardt<FunctorType>::lmder1(
345         FVectorType  &x,
346         const Scalar tol
347         )
348 {
349     n = x.size();
350     m = m_functor.values();
351 
352     /* check the input parameters for errors. */
353     if (n <= 0 || m < n || tol < 0.)
354         return LevenbergMarquardtSpace::ImproperInputParameters;
355 
356     resetParameters();
357     m_ftol = tol;
358     m_xtol = tol;
359     m_maxfev = 100*(n+1);
360 
361     return minimize(x);
362 }
363 
364 
365 template<typename FunctorType>
366 LevenbergMarquardtSpace::Status
lmdif1(FunctorType & functor,FVectorType & x,Index * nfev,const Scalar tol)367 LevenbergMarquardt<FunctorType>::lmdif1(
368         FunctorType &functor,
369         FVectorType  &x,
370         Index *nfev,
371         const Scalar tol
372         )
373 {
374     Index n = x.size();
375     Index m = functor.values();
376 
377     /* check the input parameters for errors. */
378     if (n <= 0 || m < n || tol < 0.)
379         return LevenbergMarquardtSpace::ImproperInputParameters;
380 
381     NumericalDiff<FunctorType> numDiff(functor);
382     // embedded LevenbergMarquardt
383     LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff);
384     lm.setFtol(tol);
385     lm.setXtol(tol);
386     lm.setMaxfev(200*(n+1));
387 
388     LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
389     if (nfev)
390         * nfev = lm.nfev();
391     return info;
392 }
393 
394 } // end namespace Eigen
395 
396 #endif // EIGEN_LEVENBERGMARQUARDT_H
397