14#ifndef EIGEN_LEVENBERGMARQUARDT__H
15#define EIGEN_LEVENBERGMARQUARDT__H
18#include "./InternalHeaderCheck.h"
22namespace LevenbergMarquardtSpace {
26 ImproperInputParameters = 0,
27 RelativeReductionTooSmall = 1,
28 RelativeErrorTooSmall = 2,
29 RelativeErrorAndReductionTooSmall = 3,
31 TooManyFunctionEvaluation = 5,
47template <
typename FunctorType,
typename Scalar =
double>
49 static Scalar sqrt_epsilon() {
51 return sqrt(NumTraits<Scalar>::epsilon());
55 LevenbergMarquardt(FunctorType &_functor) : functor(_functor) {
56 nfev = njev = iter = 0;
58 useExternalScaling =
false;
61 typedef DenseIndex Index;
65 : factor(Scalar(100.)),
79 typedef Matrix<Scalar, Dynamic, 1> FVectorType;
80 typedef Matrix<Scalar, Dynamic, Dynamic> JacobianType;
82 LevenbergMarquardtSpace::Status lmder1(FVectorType &x,
const Scalar tol = sqrt_epsilon());
84 LevenbergMarquardtSpace::Status minimize(FVectorType &x);
85 LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
86 LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
88 static LevenbergMarquardtSpace::Status lmdif1(FunctorType &functor, FVectorType &x, Index *nfev,
89 const Scalar tol = sqrt_epsilon());
91 LevenbergMarquardtSpace::Status lmstr1(FVectorType &x,
const Scalar tol = sqrt_epsilon());
93 LevenbergMarquardtSpace::Status minimizeOptimumStorage(FVectorType &x);
94 LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(FVectorType &x);
95 LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(FVectorType &x);
99 Parameters parameters;
100 FVectorType fvec, qtf, diag;
102 PermutationMatrix<Dynamic, Dynamic> permutation;
107 bool useExternalScaling;
109 Scalar
lm_param(
void)
const {
return par; }
112 FunctorType &functor;
115 FVectorType wa1, wa2, wa3, wa4;
118 Scalar temp, temp1, temp2;
121 Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
123 LevenbergMarquardt &operator=(
const LevenbergMarquardt &) =
delete;
126template <
typename FunctorType,
typename Scalar>
127LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::lmder1(FVectorType &x,
const Scalar tol) {
129 m = functor.values();
132 if (n <= 0 || m < n || tol < 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
135 parameters.ftol = tol;
136 parameters.xtol = tol;
137 parameters.maxfev = 100 * (n + 1);
142template <
typename FunctorType,
typename Scalar>
143LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimize(FVectorType &x) {
144 LevenbergMarquardtSpace::Status status = minimizeInit(x);
145 if (status == LevenbergMarquardtSpace::ImproperInputParameters)
return status;
147 status = minimizeOneStep(x);
148 }
while (status == LevenbergMarquardtSpace::Running);
152template <
typename FunctorType,
typename Scalar>
153LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeInit(FVectorType &x) {
155 m = functor.values();
163 if (!useExternalScaling) diag.resize(n);
164 eigen_assert((!useExternalScaling || diag.size() == n) &&
165 "When useExternalScaling is set, the caller must provide a valid 'diag'");
173 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. ||
174 parameters.maxfev <= 0 || parameters.factor <= 0.)
175 return LevenbergMarquardtSpace::ImproperInputParameters;
177 if (useExternalScaling)
178 for (Index j = 0; j < n; ++j)
179 if (diag[j] <= 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
184 if (functor(x, fvec) < 0)
return LevenbergMarquardtSpace::UserAsked;
185 fnorm = fvec.stableNorm();
191 return LevenbergMarquardtSpace::NotStarted;
194template <
typename FunctorType,
typename Scalar>
195LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeOneStep(FVectorType &x) {
199 eigen_assert(x.size() == n);
202 Index df_ret = functor.df(x, fjac);
203 if (df_ret < 0)
return LevenbergMarquardtSpace::UserAsked;
211 wa2 = fjac.colwise().blueNorm();
213 fjac = qrfac.matrixQR();
214 permutation = qrfac.colsPermutation();
219 if (!useExternalScaling)
220 for (Index j = 0; j < n; ++j) diag[j] = (wa2[j] == 0.) ? 1. : wa2[j];
224 xnorm = diag.cwiseProduct(x).stableNorm();
225 delta = parameters.factor * xnorm;
226 if (delta == 0.) delta = parameters.factor;
232 wa4.applyOnTheLeft(qrfac.householderQ().adjoint());
238 for (Index j = 0; j < n; ++j)
239 if (wa2[permutation.indices()[j]] != 0.)
240 gnorm = (std::max)(gnorm,
241 abs(fjac.col(j).head(j + 1).dot(qtf.head(j + 1) / fnorm) / wa2[permutation.indices()[j]]));
244 if (gnorm <= parameters.gtol)
return LevenbergMarquardtSpace::CosinusTooSmall;
247 if (!useExternalScaling) diag = diag.cwiseMax(wa2);
251 internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1);
256 pnorm = diag.cwiseProduct(wa1).stableNorm();
259 if (iter == 1) delta = (std::min)(delta, pnorm);
262 if (functor(wa2, wa4) < 0)
return LevenbergMarquardtSpace::UserAsked;
264 fnorm1 = wa4.stableNorm();
268 if (Scalar(.1) * fnorm1 < fnorm) actred = 1. - numext::abs2(fnorm1 / fnorm);
272 wa3.noalias() = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() * wa1);
273 temp1 = numext::abs2(wa3.stableNorm() / fnorm);
274 temp2 = numext::abs2(sqrt(par) * pnorm / fnorm);
275 prered = temp1 + temp2 / Scalar(.5);
276 dirder = -(temp1 + temp2);
281 if (prered != 0.) ratio = actred / prered;
284 if (ratio <= Scalar(.25)) {
285 if (actred >= 0.) temp = Scalar(.5);
286 if (actred < 0.) temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
287 if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) temp = Scalar(.1);
289 delta = temp * (std::min)(delta, pnorm / Scalar(.1));
291 }
else if (!(par != 0. && ratio < Scalar(.75))) {
292 delta = pnorm / Scalar(.5);
293 par = Scalar(.5) * par;
297 if (ratio >= Scalar(1e-4)) {
300 wa2 = diag.cwiseProduct(x);
302 xnorm = wa2.stableNorm();
308 if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. &&
309 delta <= parameters.xtol * xnorm)
310 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
311 if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
312 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
313 if (delta <= parameters.xtol * xnorm)
return LevenbergMarquardtSpace::RelativeErrorTooSmall;
316 if (nfev >= parameters.maxfev)
return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
318 Scalar(.5) * ratio <= 1.)
319 return LevenbergMarquardtSpace::FtolTooSmall;
323 }
while (ratio < Scalar(1e-4));
325 return LevenbergMarquardtSpace::Running;
328template <
typename FunctorType,
typename Scalar>
329LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::lmstr1(FVectorType &x,
const Scalar tol) {
331 m = functor.values();
334 if (n <= 0 || m < n || tol < 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
337 parameters.ftol = tol;
338 parameters.xtol = tol;
339 parameters.maxfev = 100 * (n + 1);
341 return minimizeOptimumStorage(x);
344template <
typename FunctorType,
typename Scalar>
345LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeOptimumStorageInit(FVectorType &x) {
347 m = functor.values();
360 if (!useExternalScaling) diag.resize(n);
361 eigen_assert((!useExternalScaling || diag.size() == n) &&
362 "When useExternalScaling is set, the caller must provide a valid 'diag'");
370 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. ||
371 parameters.maxfev <= 0 || parameters.factor <= 0.)
372 return LevenbergMarquardtSpace::ImproperInputParameters;
374 if (useExternalScaling)
375 for (Index j = 0; j < n; ++j)
376 if (diag[j] <= 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
381 if (functor(x, fvec) < 0)
return LevenbergMarquardtSpace::UserAsked;
382 fnorm = fvec.stableNorm();
388 return LevenbergMarquardtSpace::NotStarted;
391template <
typename FunctorType,
typename Scalar>
392LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeOptimumStorageOneStep(FVectorType &x) {
396 eigen_assert(x.size() == n);
408 for (i = 0; i < m; ++i) {
409 if (functor.df(x, wa3, rownb) < 0)
return LevenbergMarquardtSpace::UserAsked;
410 internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]);
418 for (j = 0; j < n; ++j) {
419 if (fjac(j, j) == 0.) sing =
true;
420 wa2[j] = fjac.col(j).head(j).stableNorm();
422 permutation.setIdentity(n);
424 wa2 = fjac.colwise().blueNorm();
428 fjac = qrfac.matrixQR();
429 wa1 = fjac.diagonal();
430 fjac.diagonal() = qrfac.hCoeffs();
431 permutation = qrfac.colsPermutation();
433 for (Index ii = 0; ii < fjac.cols(); ii++)
434 fjac.col(ii).segment(ii + 1, fjac.rows() - ii - 1) *= fjac(ii, ii);
436 for (j = 0; j < n; ++j) {
437 if (fjac(j, j) != 0.) {
439 for (i = j; i < n; ++i) sum += fjac(i, j) * qtf[i];
440 temp = -sum / fjac(j, j);
441 for (i = j; i < n; ++i) qtf[i] += fjac(i, j) * temp;
450 if (!useExternalScaling)
451 for (j = 0; j < n; ++j) diag[j] = (wa2[j] == 0.) ? 1. : wa2[j];
455 xnorm = diag.cwiseProduct(x).stableNorm();
456 delta = parameters.factor * xnorm;
457 if (delta == 0.) delta = parameters.factor;
463 for (j = 0; j < n; ++j)
464 if (wa2[permutation.indices()[j]] != 0.)
465 gnorm = (std::max)(gnorm,
466 abs(fjac.col(j).head(j + 1).dot(qtf.head(j + 1) / fnorm) / wa2[permutation.indices()[j]]));
469 if (gnorm <= parameters.gtol)
return LevenbergMarquardtSpace::CosinusTooSmall;
472 if (!useExternalScaling) diag = diag.cwiseMax(wa2);
476 internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1);
481 pnorm = diag.cwiseProduct(wa1).stableNorm();
484 if (iter == 1) delta = (std::min)(delta, pnorm);
487 if (functor(wa2, wa4) < 0)
return LevenbergMarquardtSpace::UserAsked;
489 fnorm1 = wa4.stableNorm();
493 if (Scalar(.1) * fnorm1 < fnorm) actred = 1. - numext::abs2(fnorm1 / fnorm);
497 wa3.noalias() = fjac.topLeftCorner(n, n).template triangularView<Upper>() * (permutation.inverse() * wa1);
498 temp1 = numext::abs2(wa3.stableNorm() / fnorm);
499 temp2 = numext::abs2(sqrt(par) * pnorm / fnorm);
500 prered = temp1 + temp2 / Scalar(.5);
501 dirder = -(temp1 + temp2);
506 if (prered != 0.) ratio = actred / prered;
509 if (ratio <= Scalar(.25)) {
510 if (actred >= 0.) temp = Scalar(.5);
511 if (actred < 0.) temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
512 if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1)) temp = Scalar(.1);
514 delta = temp * (std::min)(delta, pnorm / Scalar(.1));
516 }
else if (!(par != 0. && ratio < Scalar(.75))) {
517 delta = pnorm / Scalar(.5);
518 par = Scalar(.5) * par;
522 if (ratio >= Scalar(1e-4)) {
525 wa2 = diag.cwiseProduct(x);
527 xnorm = wa2.stableNorm();
533 if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. &&
534 delta <= parameters.xtol * xnorm)
535 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
536 if (abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
537 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
538 if (delta <= parameters.xtol * xnorm)
return LevenbergMarquardtSpace::RelativeErrorTooSmall;
541 if (nfev >= parameters.maxfev)
return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
543 Scalar(.5) * ratio <= 1.)
544 return LevenbergMarquardtSpace::FtolTooSmall;
548 }
while (ratio < Scalar(1e-4));
550 return LevenbergMarquardtSpace::Running;
553template <
typename FunctorType,
typename Scalar>
554LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeOptimumStorage(FVectorType &x) {
555 LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x);
556 if (status == LevenbergMarquardtSpace::ImproperInputParameters)
return status;
558 status = minimizeOptimumStorageOneStep(x);
559 }
while (status == LevenbergMarquardtSpace::Running);
563template <
typename FunctorType,
typename Scalar>
564LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::lmdif1(FunctorType &functor, FVectorType &x,
565 Index *nfev,
const Scalar tol) {
567 Index m = functor.values();
570 if (n <= 0 || m < n || tol < 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
575 lm.parameters.ftol = tol;
576 lm.parameters.xtol = tol;
577 lm.parameters.maxfev = 200 * (n + 1);
579 LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
580 if (nfev) *nfev = lm.nfev;
Performs non linear optimization over a non-linear function, using a variant of the Levenberg Marquar...
Definition LevenbergMarquardt.h:104
void resetParameters()
Definition LevenbergMarquardt.h:136
RealScalar lm_param(void) const
Definition LevenbergMarquardt.h:205
Definition NumericalDiff.h:51
Namespace containing all symbols from the Eigen library.