LevenbergMarquardt.h revision 7faaa9f3f0df9d23790277834d426c3d992ac3ba 
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
23namespace Eigen {
24namespace 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
41template <typename _Scalar, int NX=Dynamic, int NY=Dynamic>
42struct 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
68template <typename _Scalar, typename _Index>
69struct 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};
95namespace internal {
96template <typename QRSolver, typename VectorType>
97void 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  */
109template<typename _FunctorType>
110class 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
256template<typename FunctorType>
257LevenbergMarquardtSpace::Status
258LevenbergMarquardt<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
273template<typename FunctorType>
274LevenbergMarquardtSpace::Status
275LevenbergMarquardt<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
323template<typename FunctorType>
324LevenbergMarquardtSpace::Status
325LevenbergMarquardt<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
346template<typename FunctorType>
347LevenbergMarquardtSpace::Status
348LevenbergMarquardt<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