NonLinearOptimization/LevenbergMarquardt.h
Go to the documentation of this file.
1 // -*- coding: utf-8
2 // vim: set fileencoding=utf-8
3 
4 // This file is part of Eigen, a lightweight C++ template library
5 // for linear algebra.
6 //
7 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
8 //
9 // This Source Code Form is subject to the terms of the Mozilla
10 // Public License v. 2.0. If a copy of the MPL was not distributed
11 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
12 
13 #ifndef EIGEN_LEVENBERGMARQUARDT__H
14 #define EIGEN_LEVENBERGMARQUARDT__H
15 
16 namespace Eigen {
17 
18 namespace LevenbergMarquardtSpace {
19  enum Status {
20  NotStarted = -2,
21  Running = -1,
26  CosinusTooSmall = 4,
28  FtolTooSmall = 6,
29  XtolTooSmall = 7,
30  GtolTooSmall = 8,
31  UserAsked = 9
32  };
33 }
34 
35 
36 
45 template<typename FunctorType, typename Scalar=double>
47 {
49  {
50  using std::sqrt;
52  }
53 
54 public:
56  : functor(_functor) { nfev = njev = iter = 0; fnorm = gnorm = 0.; useExternalScaling=false; }
57 
58  typedef DenseIndex Index;
59 
60  struct Parameters {
62  : factor(Scalar(100.))
63  , maxfev(400)
64  , ftol(sqrt_epsilon())
65  , xtol(sqrt_epsilon())
66  , gtol(Scalar(0.))
67  , epsfcn(Scalar(0.)) {}
69  Index maxfev; // maximum number of function evaluation
74  };
75 
78 
80  FVectorType &x,
81  const Scalar tol = sqrt_epsilon()
82  );
83 
84  LevenbergMarquardtSpace::Status minimize(FVectorType &x);
85  LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
86  LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
87 
88  static LevenbergMarquardtSpace::Status lmdif1(
89  FunctorType &functor,
90  FVectorType &x,
91  Index *nfev,
92  const Scalar tol = sqrt_epsilon()
93  );
94 
96  FVectorType &x,
97  const Scalar tol = sqrt_epsilon()
98  );
99 
100  LevenbergMarquardtSpace::Status minimizeOptimumStorage(FVectorType &x);
101  LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(FVectorType &x);
102  LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(FVectorType &x);
103 
105 
107  FVectorType fvec, qtf, diag;
108  JacobianType fjac;
110  Index nfev;
111  Index njev;
112  Index iter;
113  Scalar fnorm, gnorm;
115 
116  Scalar lm_param(void) { return par; }
117 private:
118 
120  Index n;
121  Index m;
122  FVectorType wa1, wa2, wa3, wa4;
123 
124  Scalar par, sum;
125  Scalar temp, temp1, temp2;
128  Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
129 
130  LevenbergMarquardt& operator=(const LevenbergMarquardt&);
131 };
132 
133 template<typename FunctorType, typename Scalar>
136  FVectorType &x,
137  const Scalar tol
138  )
139 {
140  n = x.size();
141  m = functor.values();
142 
143  /* check the input parameters for errors. */
144  if (n <= 0 || m < n || tol < 0.)
146 
147  resetParameters();
148  parameters.ftol = tol;
149  parameters.xtol = tol;
150  parameters.maxfev = 100*(n+1);
151 
152  return minimize(x);
153 }
154 
155 
156 template<typename FunctorType, typename Scalar>
159 {
160  LevenbergMarquardtSpace::Status status = minimizeInit(x);
162  return status;
163  do {
164  status = minimizeOneStep(x);
165  } while (status==LevenbergMarquardtSpace::Running);
166  return status;
167 }
168 
169 template<typename FunctorType, typename Scalar>
172 {
173  n = x.size();
174  m = functor.values();
175 
176  wa1.resize(n); wa2.resize(n); wa3.resize(n);
177  wa4.resize(m);
178  fvec.resize(m);
179  fjac.resize(m, n);
180  if (!useExternalScaling)
181  diag.resize(n);
182  eigen_assert( (!useExternalScaling || diag.size()==n) && "When useExternalScaling is set, the caller must provide a valid 'diag'");
183  qtf.resize(n);
184 
185  /* Function Body */
186  nfev = 0;
187  njev = 0;
188 
189  /* check the input parameters for errors. */
190  if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
192 
193  if (useExternalScaling)
194  for (Index j = 0; j < n; ++j)
195  if (diag[j] <= 0.)
197 
198  /* evaluate the function at the starting point */
199  /* and calculate its norm. */
200  nfev = 1;
201  if ( functor(x, fvec) < 0)
203  fnorm = fvec.stableNorm();
204 
205  /* initialize levenberg-marquardt parameter and iteration counter. */
206  par = 0.;
207  iter = 1;
208 
210 }
211 
212 template<typename FunctorType, typename Scalar>
215 {
216  using std::abs;
217  using std::sqrt;
218 
219  eigen_assert(x.size()==n); // check the caller is not cheating us
220 
221  /* calculate the jacobian matrix. */
222  Index df_ret = functor.df(x, fjac);
223  if (df_ret<0)
225  if (df_ret>0)
226  // numerical diff, we evaluated the function df_ret times
227  nfev += df_ret;
228  else njev++;
229 
230  /* compute the qr factorization of the jacobian. */
231  wa2 = fjac.colwise().blueNorm();
233  fjac = qrfac.matrixQR();
234  permutation = qrfac.colsPermutation();
235 
236  /* on the first iteration and if external scaling is not used, scale according */
237  /* to the norms of the columns of the initial jacobian. */
238  if (iter == 1) {
239  if (!useExternalScaling)
240  for (Index j = 0; j < n; ++j)
241  diag[j] = (wa2[j]==0.)? 1. : wa2[j];
242 
243  /* on the first iteration, calculate the norm of the scaled x */
244  /* and initialize the step bound delta. */
245  xnorm = diag.cwiseProduct(x).stableNorm();
246  delta = parameters.factor * xnorm;
247  if (delta == 0.)
248  delta = parameters.factor;
249  }
250 
251  /* form (q transpose)*fvec and store the first n components in */
252  /* qtf. */
253  wa4 = fvec;
254  wa4.applyOnTheLeft(qrfac.householderQ().adjoint());
255  qtf = wa4.head(n);
256 
257  /* compute the norm of the scaled gradient. */
258  gnorm = 0.;
259  if (fnorm != 0.)
260  for (Index j = 0; j < n; ++j)
261  if (wa2[permutation.indices()[j]] != 0.)
262  gnorm = (std::max)(gnorm, abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
263 
264  /* test for convergence of the gradient norm. */
265  if (gnorm <= parameters.gtol)
267 
268  /* rescale if necessary. */
269  if (!useExternalScaling)
270  diag = diag.cwiseMax(wa2);
271 
272  do {
273 
274  /* determine the levenberg-marquardt parameter. */
275  internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1);
276 
277  /* store the direction p and x + p. calculate the norm of p. */
278  wa1 = -wa1;
279  wa2 = x + wa1;
280  pnorm = diag.cwiseProduct(wa1).stableNorm();
281 
282  /* on the first iteration, adjust the initial step bound. */
283  if (iter == 1)
284  delta = (std::min)(delta,pnorm);
285 
286  /* evaluate the function at x + p and calculate its norm. */
287  if ( functor(wa2, wa4) < 0)
289  ++nfev;
290  fnorm1 = wa4.stableNorm();
291 
292  /* compute the scaled actual reduction. */
293  actred = -1.;
294  if (Scalar(.1) * fnorm1 < fnorm)
295  actred = 1. - numext::abs2(fnorm1 / fnorm);
296 
297  /* compute the scaled predicted reduction and */
298  /* the scaled directional derivative. */
299  wa3 = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() *wa1);
300  temp1 = numext::abs2(wa3.stableNorm() / fnorm);
301  temp2 = numext::abs2(sqrt(par) * pnorm / fnorm);
302  prered = temp1 + temp2 / Scalar(.5);
303  dirder = -(temp1 + temp2);
304 
305  /* compute the ratio of the actual to the predicted */
306  /* reduction. */
307  ratio = 0.;
308  if (prered != 0.)
309  ratio = actred / prered;
310 
311  /* update the step bound. */
312  if (ratio <= Scalar(.25)) {
313  if (actred >= 0.)
314  temp = Scalar(.5);
315  if (actred < 0.)
316  temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
317  if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
318  temp = Scalar(.1);
319  /* Computing MIN */
320  delta = temp * (std::min)(delta, pnorm / Scalar(.1));
321  par /= temp;
322  } else if (!(par != 0. && ratio < Scalar(.75))) {
323  delta = pnorm / Scalar(.5);
324  par = Scalar(.5) * par;
325  }
326 
327  /* test for successful iteration. */
328  if (ratio >= Scalar(1e-4)) {
329  /* successful iteration. update x, fvec, and their norms. */
330  x = wa2;
331  wa2 = diag.cwiseProduct(x);
332  fvec = wa4;
333  xnorm = wa2.stableNorm();
334  fnorm = fnorm1;
335  ++iter;
336  }
337 
338  /* tests for convergence. */
339  if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
341  if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
343  if (delta <= parameters.xtol * xnorm)
345 
346  /* tests for termination and stringent tolerances. */
347  if (nfev >= parameters.maxfev)
349  if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
351  if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
353  if (gnorm <= NumTraits<Scalar>::epsilon())
355 
356  } while (ratio < Scalar(1e-4));
357 
359 }
360 
361 template<typename FunctorType, typename Scalar>
364  FVectorType &x,
365  const Scalar tol
366  )
367 {
368  n = x.size();
369  m = functor.values();
370 
371  /* check the input parameters for errors. */
372  if (n <= 0 || m < n || tol < 0.)
374 
375  resetParameters();
376  parameters.ftol = tol;
377  parameters.xtol = tol;
378  parameters.maxfev = 100*(n+1);
379 
380  return minimizeOptimumStorage(x);
381 }
382 
383 template<typename FunctorType, typename Scalar>
386 {
387  n = x.size();
388  m = functor.values();
389 
390  wa1.resize(n); wa2.resize(n); wa3.resize(n);
391  wa4.resize(m);
392  fvec.resize(m);
393  // Only R is stored in fjac. Q is only used to compute 'qtf', which is
394  // Q.transpose()*rhs. qtf will be updated using givens rotation,
395  // instead of storing them in Q.
396  // The purpose it to only use a nxn matrix, instead of mxn here, so
397  // that we can handle cases where m>>n :
398  fjac.resize(n, n);
399  if (!useExternalScaling)
400  diag.resize(n);
401  eigen_assert( (!useExternalScaling || diag.size()==n) && "When useExternalScaling is set, the caller must provide a valid 'diag'");
402  qtf.resize(n);
403 
404  /* Function Body */
405  nfev = 0;
406  njev = 0;
407 
408  /* check the input parameters for errors. */
409  if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
411 
412  if (useExternalScaling)
413  for (Index j = 0; j < n; ++j)
414  if (diag[j] <= 0.)
416 
417  /* evaluate the function at the starting point */
418  /* and calculate its norm. */
419  nfev = 1;
420  if ( functor(x, fvec) < 0)
422  fnorm = fvec.stableNorm();
423 
424  /* initialize levenberg-marquardt parameter and iteration counter. */
425  par = 0.;
426  iter = 1;
427 
429 }
430 
431 
432 template<typename FunctorType, typename Scalar>
435 {
436  using std::abs;
437  using std::sqrt;
438 
439  eigen_assert(x.size()==n); // check the caller is not cheating us
440 
441  Index i, j;
442  bool sing;
443 
444  /* compute the qr factorization of the jacobian matrix */
445  /* calculated one row at a time, while simultaneously */
446  /* forming (q transpose)*fvec and storing the first */
447  /* n components in qtf. */
448  qtf.fill(0.);
449  fjac.fill(0.);
450  Index rownb = 2;
451  for (i = 0; i < m; ++i) {
452  if (functor.df(x, wa3, rownb) < 0) return LevenbergMarquardtSpace::UserAsked;
453  internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]);
454  ++rownb;
455  }
456  ++njev;
457 
458  /* if the jacobian is rank deficient, call qrfac to */
459  /* reorder its columns and update the components of qtf. */
460  sing = false;
461  for (j = 0; j < n; ++j) {
462  if (fjac(j,j) == 0.)
463  sing = true;
464  wa2[j] = fjac.col(j).head(j).stableNorm();
465  }
466  permutation.setIdentity(n);
467  if (sing) {
468  wa2 = fjac.colwise().blueNorm();
469  // TODO We have no unit test covering this code path, do not modify
470  // until it is carefully tested
472  fjac = qrfac.matrixQR();
473  wa1 = fjac.diagonal();
474  fjac.diagonal() = qrfac.hCoeffs();
475  permutation = qrfac.colsPermutation();
476  // TODO : avoid this:
477  for(Index ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii); // rescale vectors
478 
479  for (j = 0; j < n; ++j) {
480  if (fjac(j,j) != 0.) {
481  sum = 0.;
482  for (i = j; i < n; ++i)
483  sum += fjac(i,j) * qtf[i];
484  temp = -sum / fjac(j,j);
485  for (i = j; i < n; ++i)
486  qtf[i] += fjac(i,j) * temp;
487  }
488  fjac(j,j) = wa1[j];
489  }
490  }
491 
492  /* on the first iteration and if external scaling is not used, scale according */
493  /* to the norms of the columns of the initial jacobian. */
494  if (iter == 1) {
495  if (!useExternalScaling)
496  for (j = 0; j < n; ++j)
497  diag[j] = (wa2[j]==0.)? 1. : wa2[j];
498 
499  /* on the first iteration, calculate the norm of the scaled x */
500  /* and initialize the step bound delta. */
501  xnorm = diag.cwiseProduct(x).stableNorm();
502  delta = parameters.factor * xnorm;
503  if (delta == 0.)
504  delta = parameters.factor;
505  }
506 
507  /* compute the norm of the scaled gradient. */
508  gnorm = 0.;
509  if (fnorm != 0.)
510  for (j = 0; j < n; ++j)
511  if (wa2[permutation.indices()[j]] != 0.)
512  gnorm = (std::max)(gnorm, abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
513 
514  /* test for convergence of the gradient norm. */
515  if (gnorm <= parameters.gtol)
517 
518  /* rescale if necessary. */
519  if (!useExternalScaling)
520  diag = diag.cwiseMax(wa2);
521 
522  do {
523 
524  /* determine the levenberg-marquardt parameter. */
525  internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1);
526 
527  /* store the direction p and x + p. calculate the norm of p. */
528  wa1 = -wa1;
529  wa2 = x + wa1;
530  pnorm = diag.cwiseProduct(wa1).stableNorm();
531 
532  /* on the first iteration, adjust the initial step bound. */
533  if (iter == 1)
534  delta = (std::min)(delta,pnorm);
535 
536  /* evaluate the function at x + p and calculate its norm. */
537  if ( functor(wa2, wa4) < 0)
539  ++nfev;
540  fnorm1 = wa4.stableNorm();
541 
542  /* compute the scaled actual reduction. */
543  actred = -1.;
544  if (Scalar(.1) * fnorm1 < fnorm)
545  actred = 1. - numext::abs2(fnorm1 / fnorm);
546 
547  /* compute the scaled predicted reduction and */
548  /* the scaled directional derivative. */
549  wa3 = fjac.topLeftCorner(n,n).template triangularView<Upper>() * (permutation.inverse() * wa1);
550  temp1 = numext::abs2(wa3.stableNorm() / fnorm);
551  temp2 = numext::abs2(sqrt(par) * pnorm / fnorm);
552  prered = temp1 + temp2 / Scalar(.5);
553  dirder = -(temp1 + temp2);
554 
555  /* compute the ratio of the actual to the predicted */
556  /* reduction. */
557  ratio = 0.;
558  if (prered != 0.)
559  ratio = actred / prered;
560 
561  /* update the step bound. */
562  if (ratio <= Scalar(.25)) {
563  if (actred >= 0.)
564  temp = Scalar(.5);
565  if (actred < 0.)
566  temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
567  if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
568  temp = Scalar(.1);
569  /* Computing MIN */
570  delta = temp * (std::min)(delta, pnorm / Scalar(.1));
571  par /= temp;
572  } else if (!(par != 0. && ratio < Scalar(.75))) {
573  delta = pnorm / Scalar(.5);
574  par = Scalar(.5) * par;
575  }
576 
577  /* test for successful iteration. */
578  if (ratio >= Scalar(1e-4)) {
579  /* successful iteration. update x, fvec, and their norms. */
580  x = wa2;
581  wa2 = diag.cwiseProduct(x);
582  fvec = wa4;
583  xnorm = wa2.stableNorm();
584  fnorm = fnorm1;
585  ++iter;
586  }
587 
588  /* tests for convergence. */
589  if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
591  if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
593  if (delta <= parameters.xtol * xnorm)
595 
596  /* tests for termination and stringent tolerances. */
597  if (nfev >= parameters.maxfev)
599  if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
601  if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
603  if (gnorm <= NumTraits<Scalar>::epsilon())
605 
606  } while (ratio < Scalar(1e-4));
607 
609 }
610 
611 template<typename FunctorType, typename Scalar>
614 {
615  LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x);
617  return status;
618  do {
619  status = minimizeOptimumStorageOneStep(x);
620  } while (status==LevenbergMarquardtSpace::Running);
621  return status;
622 }
623 
624 template<typename FunctorType, typename Scalar>
627  FunctorType &functor,
628  FVectorType &x,
629  Index *nfev,
630  const Scalar tol
631  )
632 {
633  Index n = x.size();
634  Index m = functor.values();
635 
636  /* check the input parameters for errors. */
637  if (n <= 0 || m < n || tol < 0.)
639 
640  NumericalDiff<FunctorType> numDiff(functor);
641  // embedded LevenbergMarquardt
643  lm.parameters.ftol = tol;
644  lm.parameters.xtol = tol;
645  lm.parameters.maxfev = 200*(n+1);
646 
648  if (nfev)
649  * nfev = lm.nfev;
650  return info;
651 }
652 
653 } // end namespace Eigen
654 
655 #endif // EIGEN_LEVENBERGMARQUARDT__H
656 
657 //vim: ai ts=4 sts=4 et sw=4
Matrix3f m
LevenbergMarquardtSpace::Status minimize(FVectorType &x)
Matrix< Scalar, Dynamic, Dynamic > JacobianType
SCALAR Scalar
Definition: bench_gemm.cpp:46
static LevenbergMarquardtSpace::Status lmdif1(FunctorType &functor, FVectorType &x, Index *nfev, const Scalar tol=std::sqrt(NumTraits< Scalar >::epsilon()))
#define max(a, b)
Definition: datatypes.h:20
LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x)
Definition: LMonestep.h:21
Performs non linear optimization over a non-linear function, using a variant of the Levenberg Marquar...
Matrix diag(const std::vector< Matrix > &Hs)
Definition: Matrix.cpp:206
#define min(a, b)
Definition: datatypes.h:19
const HCoeffsType & hCoeffs() const
PermutationMatrix< Dynamic, Dynamic > permutation
int n
LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(FVectorType &x)
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
iterator iter(handle obj)
Definition: pytypes.h:2273
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:232
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size ratio
else if n * info
LevenbergMarquardtSpace::Status lmstr1(FVectorType &x, const Scalar tol=sqrt_epsilon())
Householder rank-revealing QR decomposition of a matrix with column-pivoting.
#define eigen_assert(x)
Definition: Macros.h:1037
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static ConjugateGradientParameters parameters
LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(FVectorType &x)
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: Meta.h:66
LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x)
LevenbergMarquardtSpace::Status minimizeOptimumStorage(FVectorType &x)
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
const G double tol
Definition: Group.h:86
LevenbergMarquardtSpace::Status lmder1(FVectorType &x, const Scalar tol=std::sqrt(NumTraits< Scalar >::epsilon()))
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
const MatrixType & matrixQR() const
#define abs(x)
Definition: datatypes.h:17
const PermutationType & colsPermutation() const
EIGEN_DEVICE_FUNC bool abs2(bool x)
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:33