All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Groups Pages
LevenbergMarquardt/LevenbergMarquardt.h
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 
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 
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 
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 };
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  }
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:
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 
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 
158  void setXtol(RealScalar xtol) { m_xtol = xtol; }
159 
161  void setFtol(RealScalar ftol) { m_ftol = ftol; }
162 
164  void setGtol(RealScalar gtol) { m_gtol = gtol; }
165 
167  void setFactor(RealScalar factor) { m_factor = factor; }
168 
170  void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; }
171 
173  void setMaxfev(Index maxfev) {m_maxfev = maxfev; }
174 
176  void setExternalScaling(bool value) {m_useExternalScaling = value; }
177 
179  RealScalar xtol() const {return m_xtol; }
180 
182  RealScalar ftol() const {return m_ftol; }
183 
185  RealScalar gtol() const {return m_gtol; }
186 
188  RealScalar factor() const {return m_factor; }
189 
191  RealScalar epsilon() const {return m_epsfcn; }
192 
194  Index maxfev() const {return m_maxfev; }
195 
197  FVectorType& diag() {return m_diag; }
198 
200  Index iterations() { return m_iter; }
201 
203  Index nfev() { return m_nfev; }
204 
206  Index njev() { return m_njev; }
207 
209  RealScalar fnorm() {return m_fnorm; }
210 
212  RealScalar gnorm() {return m_gnorm; }
213 
215  RealScalar lm_param(void) { return m_par; }
216 
219  FVectorType& fvec() {return m_fvec; }
220 
223  JacobianType& jacobian() {return m_fjac; }
224 
228  JacobianType& matrixR() {return m_rfactor; }
229 
232  PermutationType permutation() {return m_permutation; }
233 
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
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
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
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
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
Index maxfev() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:194
RealScalar gtol() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:185
RealScalar fnorm()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:209
Performs non linear optimization over a non-linear function, using a variant of the Levenberg Marquar...
Definition: LevenbergMarquardt/LevenbergMarquardt.h:110
RealScalar ftol() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:182
JacobianType & jacobian()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:223
FVectorType & fvec()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:219
RealScalar epsilon() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:191
PermutationType permutation()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:232
void setFtol(RealScalar ftol)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:161
void setEpsilon(RealScalar epsfcn)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:170
RealScalar xtol() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:179
void setMaxfev(Index maxfev)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:173
Index nfev()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:203
Index njev()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:206
RealScalar lm_param(void)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:215
ComputationInfo info() const
Reports whether the minimization was successful.
Definition: LevenbergMarquardt/LevenbergMarquardt.h:243
void resetParameters()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:145
RealScalar factor() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:188
void setFactor(RealScalar factor)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:167
FVectorType & diag()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:197
void setXtol(RealScalar xtol)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:158
JacobianType & matrixR()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:228
Index iterations()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:200
void setGtol(RealScalar gtol)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:164
void setExternalScaling(bool value)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:176
RealScalar gnorm()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:212