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 QRSolver::StorageIndex PermIndex;
119    typedef Matrix<Scalar,Dynamic,1> FVectorType;
120    typedef PermutationMatrix<Dynamic,Dynamic> PermutationType;
121  public:
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 */
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*/
158    void setXtol(RealScalar xtol) { m_xtol = xtol; }
159
160    /** Sets the tolerance for the norm of the vector function*/
161    void setFtol(RealScalar ftol) { m_ftol = ftol; }
162
163    /** Sets the tolerance for the norm of the gradient of the error vector*/
164    void setGtol(RealScalar gtol) { m_gtol = gtol; }
165
166    /** Sets the step bound for the diagonal shift */
167    void setFactor(RealScalar factor) { m_factor = factor; }
168
169    /** Sets the error precision  */
170    void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; }
171
172    /** Sets the maximum number of function evaluation */
173    void setMaxfev(Index maxfev) {m_maxfev = maxfev; }
174
175    /** Use an external Scaling. If set to true, pass a nonzero diagonal to diag() */
176    void setExternalScaling(bool value) {m_useExternalScaling  = value; }
177
178    /** \returns the tolerance for the norm of the solution vector */
179    RealScalar xtol() const {return m_xtol; }
180
181    /** \returns the tolerance for the norm of the vector function */
182    RealScalar ftol() const {return m_ftol; }
183
184    /** \returns the tolerance for the norm of the gradient of the error vector */
185    RealScalar gtol() const {return m_gtol; }
186
187    /** \returns the step bound for the diagonal shift */
188    RealScalar factor() const {return m_factor; }
189
190    /** \returns the error precision */
191    RealScalar epsilon() const {return m_epsfcn; }
192
193    /** \returns the maximum number of function evaluation */
194    Index maxfev() const {return m_maxfev; }
195
196    /** \returns a reference to the diagonal of the jacobian */
197    FVectorType& diag() {return m_diag; }
198
199    /** \returns the number of iterations performed */
200    Index iterations() { return m_iter; }
201
202    /** \returns the number of functions evaluation */
203    Index nfev() { return m_nfev; }
204
205    /** \returns the number of jacobian evaluation */
206    Index njev() { return m_njev; }
207
208    /** \returns the norm of current vector function */
209    RealScalar fnorm() {return m_fnorm; }
210
211    /** \returns the norm of the gradient of the error */
212    RealScalar gnorm() {return m_gnorm; }
213
214    /** \returns the LevenbergMarquardt parameter */
215    RealScalar lm_param(void) { return m_par; }
216
217    /** \returns a reference to the  current vector function
218     */
219    FVectorType& fvec() {return m_fvec; }
220
221    /** \returns a reference to the matrix where the current Jacobian matrix is stored
222     */
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     */
228    JacobianType& matrixR() {return m_rfactor; }
229
230    /** the permutation used in the QR factorization
231     */
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     */
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
275template<typename FunctorType>
276LevenbergMarquardtSpace::Status
277LevenbergMarquardt<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
292template<typename FunctorType>
293LevenbergMarquardtSpace::Status
294LevenbergMarquardt<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
342template<typename FunctorType>
343LevenbergMarquardtSpace::Status
344LevenbergMarquardt<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
365template<typename FunctorType>
366LevenbergMarquardtSpace::Status
367LevenbergMarquardt<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