13#ifndef EIGEN_LEVENBERGMARQUARDT__H
14#define EIGEN_LEVENBERGMARQUARDT__H
17#include "./InternalHeaderCheck.h"
21namespace LevenbergMarquardtSpace {
25 ImproperInputParameters = 0,
26 RelativeReductionTooSmall = 1,
27 RelativeErrorTooSmall = 2,
28 RelativeErrorAndReductionTooSmall = 3,
30 TooManyFunctionEvaluation = 5,
46template <
typename FunctorType,
typename Scalar =
double>
48 static Scalar sqrt_epsilon() {
50 return sqrt(NumTraits<Scalar>::epsilon());
54 LevenbergMarquardt(FunctorType &_functor) : functor(_functor) {
55 nfev = njev = iter = 0;
57 useExternalScaling =
false;
60 typedef DenseIndex Index;
64 : factor(Scalar(100.)),
78 typedef Matrix<Scalar, Dynamic, 1> FVectorType;
79 typedef Matrix<Scalar, Dynamic, Dynamic> JacobianType;
81 LevenbergMarquardtSpace::Status lmder1(FVectorType &x,
const Scalar tol = sqrt_epsilon());
83 LevenbergMarquardtSpace::Status minimize(FVectorType &x);
84 LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
85 LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
87 static LevenbergMarquardtSpace::Status lmdif1(FunctorType &functor, FVectorType &x, Index *nfev,
88 const Scalar tol = sqrt_epsilon());
90 LevenbergMarquardtSpace::Status lmstr1(FVectorType &x,
const Scalar tol = sqrt_epsilon());
92 LevenbergMarquardtSpace::Status minimizeOptimumStorage(FVectorType &x);
93 LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(FVectorType &x);
94 LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(FVectorType &x);
98 Parameters parameters;
99 FVectorType fvec, qtf, diag;
101 PermutationMatrix<Dynamic, Dynamic> permutation;
106 bool useExternalScaling;
108 Scalar
lm_param(
void) {
return par; }
111 FunctorType &functor;
114 FVectorType wa1, wa2, wa3, wa4;
117 Scalar temp, temp1, temp2;
120 Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
122 LevenbergMarquardt &operator=(
const LevenbergMarquardt &);
125template <
typename FunctorType,
typename Scalar>
126LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::lmder1(FVectorType &x,
const Scalar tol) {
128 m = functor.values();
131 if (n <= 0 || m < n || tol < 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
134 parameters.ftol = tol;
135 parameters.xtol = tol;
136 parameters.maxfev = 100 * (n + 1);
141template <
typename FunctorType,
typename Scalar>
142LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimize(FVectorType &x) {
143 LevenbergMarquardtSpace::Status status = minimizeInit(x);
144 if (status == LevenbergMarquardtSpace::ImproperInputParameters)
return status;
146 status = minimizeOneStep(x);
147 }
while (status == LevenbergMarquardtSpace::Running);
151template <
typename FunctorType,
typename Scalar>
152LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeInit(FVectorType &x) {
154 m = functor.values();
162 if (!useExternalScaling) diag.resize(n);
163 eigen_assert((!useExternalScaling || diag.size() == n) &&
164 "When useExternalScaling is set, the caller must provide a valid 'diag'");
172 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. ||
173 parameters.maxfev <= 0 || parameters.factor <= 0.)
174 return LevenbergMarquardtSpace::ImproperInputParameters;
176 if (useExternalScaling)
177 for (
Index j = 0; j < n; ++j)
178 if (diag[j] <= 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
183 if (functor(x, fvec) < 0)
return LevenbergMarquardtSpace::UserAsked;
184 fnorm = fvec.stableNorm();
190 return LevenbergMarquardtSpace::NotStarted;
193template <
typename FunctorType,
typename Scalar>
194LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeOneStep(FVectorType &x) {
198 eigen_assert(x.size() == n);
201 Index df_ret = functor.df(x, fjac);
202 if (df_ret < 0)
return LevenbergMarquardtSpace::UserAsked;
210 wa2 = fjac.colwise().blueNorm();
212 fjac = qrfac.matrixQR();
213 permutation = qrfac.colsPermutation();
218 if (!useExternalScaling)
219 for (
Index j = 0; j < n; ++j) diag[j] = (wa2[j] == 0.) ? 1. : wa2[j];
223 xnorm = diag.cwiseProduct(x).stableNorm();
224 delta = parameters.factor * xnorm;
225 if (delta == 0.) delta = parameters.factor;
231 wa4.applyOnTheLeft(qrfac.householderQ().adjoint());
237 for (
Index j = 0; j < n; ++j)
238 if (wa2[permutation.indices()[j]] != 0.)
239 gnorm = (std::max)(gnorm,
240 abs(fjac.col(j).head(j + 1).dot(qtf.head(j + 1) / fnorm) / wa2[permutation.indices()[j]]));
243 if (gnorm <= parameters.gtol)
return LevenbergMarquardtSpace::CosinusTooSmall;
246 if (!useExternalScaling) diag = diag.cwiseMax(wa2);
250 internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1);
255 pnorm = diag.cwiseProduct(wa1).stableNorm();
258 if (iter == 1) delta = (std::min)(delta, pnorm);
261 if (functor(wa2, wa4) < 0)
return LevenbergMarquardtSpace::UserAsked;
263 fnorm1 = wa4.stableNorm();
267 if (
Scalar(.1) * fnorm1 < fnorm) actred = 1. - numext::abs2(fnorm1 / fnorm);
271 wa3.noalias() = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().
inverse() * wa1);
272 temp1 = numext::abs2(wa3.stableNorm() / fnorm);
273 temp2 = numext::abs2(
sqrt(par) * pnorm / fnorm);
274 prered = temp1 + temp2 /
Scalar(.5);
275 dirder = -(temp1 + temp2);
280 if (prered != 0.) ratio = actred / prered;
283 if (ratio <=
Scalar(.25)) {
284 if (actred >= 0.) temp =
Scalar(.5);
285 if (actred < 0.) temp =
Scalar(.5) * dirder / (dirder +
Scalar(.5) * actred);
288 delta = temp * (std::min)(delta, pnorm /
Scalar(.1));
290 }
else if (!(par != 0. && ratio <
Scalar(.75))) {
291 delta = pnorm /
Scalar(.5);
296 if (ratio >=
Scalar(1e-4)) {
299 wa2 = diag.cwiseProduct(x);
301 xnorm = wa2.stableNorm();
307 if (
abs(actred) <= parameters.ftol && prered <= parameters.ftol &&
Scalar(.5) * ratio <= 1. &&
308 delta <= parameters.xtol * xnorm)
309 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
310 if (
abs(actred) <= parameters.ftol && prered <= parameters.ftol &&
Scalar(.5) * ratio <= 1.)
311 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
312 if (delta <= parameters.xtol * xnorm)
return LevenbergMarquardtSpace::RelativeErrorTooSmall;
315 if (nfev >= parameters.maxfev)
return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
318 return LevenbergMarquardtSpace::FtolTooSmall;
322 }
while (ratio <
Scalar(1e-4));
324 return LevenbergMarquardtSpace::Running;
327template <
typename FunctorType,
typename Scalar>
328LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::lmstr1(FVectorType &x,
const Scalar tol) {
330 m = functor.values();
333 if (n <= 0 || m < n || tol < 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
336 parameters.ftol = tol;
337 parameters.xtol = tol;
338 parameters.maxfev = 100 * (n + 1);
340 return minimizeOptimumStorage(x);
343template <
typename FunctorType,
typename Scalar>
344LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeOptimumStorageInit(FVectorType &x) {
346 m = functor.values();
359 if (!useExternalScaling) diag.resize(n);
360 eigen_assert((!useExternalScaling || diag.size() == n) &&
361 "When useExternalScaling is set, the caller must provide a valid 'diag'");
369 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. ||
370 parameters.maxfev <= 0 || parameters.factor <= 0.)
371 return LevenbergMarquardtSpace::ImproperInputParameters;
373 if (useExternalScaling)
374 for (
Index j = 0; j < n; ++j)
375 if (diag[j] <= 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
380 if (functor(x, fvec) < 0)
return LevenbergMarquardtSpace::UserAsked;
381 fnorm = fvec.stableNorm();
387 return LevenbergMarquardtSpace::NotStarted;
390template <
typename FunctorType,
typename Scalar>
391LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeOptimumStorageOneStep(FVectorType &x) {
395 eigen_assert(x.size() == n);
407 for (i = 0; i < m; ++i) {
408 if (functor.df(x, wa3, rownb) < 0)
return LevenbergMarquardtSpace::UserAsked;
409 internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]);
417 for (j = 0; j < n; ++j) {
418 if (fjac(j, j) == 0.) sing =
true;
419 wa2[j] = fjac.col(j).head(j).stableNorm();
421 permutation.setIdentity(n);
423 wa2 = fjac.colwise().blueNorm();
427 fjac = qrfac.matrixQR();
428 wa1 = fjac.diagonal();
429 fjac.diagonal() = qrfac.hCoeffs();
430 permutation = qrfac.colsPermutation();
432 for (
Index ii = 0; ii < fjac.cols(); ii++)
433 fjac.col(ii).segment(ii + 1, fjac.rows() - ii - 1) *= fjac(ii, ii);
435 for (j = 0; j < n; ++j) {
436 if (fjac(j, j) != 0.) {
438 for (i = j; i < n; ++i) sum += fjac(i, j) * qtf[i];
439 temp = -sum / fjac(j, j);
440 for (i = j; i < n; ++i) qtf[i] += fjac(i, j) * temp;
449 if (!useExternalScaling)
450 for (j = 0; j < n; ++j) diag[j] = (wa2[j] == 0.) ? 1. : wa2[j];
454 xnorm = diag.cwiseProduct(x).stableNorm();
455 delta = parameters.factor * xnorm;
456 if (delta == 0.) delta = parameters.factor;
462 for (j = 0; j < n; ++j)
463 if (wa2[permutation.indices()[j]] != 0.)
464 gnorm = (std::max)(gnorm,
465 abs(fjac.col(j).head(j + 1).dot(qtf.head(j + 1) / fnorm) / wa2[permutation.indices()[j]]));
468 if (gnorm <= parameters.gtol)
return LevenbergMarquardtSpace::CosinusTooSmall;
471 if (!useExternalScaling) diag = diag.cwiseMax(wa2);
475 internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1);
480 pnorm = diag.cwiseProduct(wa1).stableNorm();
483 if (iter == 1) delta = (std::min)(delta, pnorm);
486 if (functor(wa2, wa4) < 0)
return LevenbergMarquardtSpace::UserAsked;
488 fnorm1 = wa4.stableNorm();
492 if (
Scalar(.1) * fnorm1 < fnorm) actred = 1. - numext::abs2(fnorm1 / fnorm);
496 wa3 = fjac.topLeftCorner(n, n).template triangularView<Upper>() * (permutation.inverse() * wa1);
497 temp1 = numext::abs2(wa3.stableNorm() / fnorm);
498 temp2 = numext::abs2(
sqrt(par) * pnorm / fnorm);
499 prered = temp1 + temp2 /
Scalar(.5);
500 dirder = -(temp1 + temp2);
505 if (prered != 0.) ratio = actred / prered;
508 if (ratio <=
Scalar(.25)) {
509 if (actred >= 0.) temp =
Scalar(.5);
510 if (actred < 0.) temp =
Scalar(.5) * dirder / (dirder +
Scalar(.5) * actred);
513 delta = temp * (std::min)(delta, pnorm /
Scalar(.1));
515 }
else if (!(par != 0. && ratio <
Scalar(.75))) {
516 delta = pnorm /
Scalar(.5);
521 if (ratio >=
Scalar(1e-4)) {
524 wa2 = diag.cwiseProduct(x);
526 xnorm = wa2.stableNorm();
532 if (
abs(actred) <= parameters.ftol && prered <= parameters.ftol &&
Scalar(.5) * ratio <= 1. &&
533 delta <= parameters.xtol * xnorm)
534 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
535 if (
abs(actred) <= parameters.ftol && prered <= parameters.ftol &&
Scalar(.5) * ratio <= 1.)
536 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
537 if (delta <= parameters.xtol * xnorm)
return LevenbergMarquardtSpace::RelativeErrorTooSmall;
540 if (nfev >= parameters.maxfev)
return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
543 return LevenbergMarquardtSpace::FtolTooSmall;
547 }
while (ratio <
Scalar(1e-4));
549 return LevenbergMarquardtSpace::Running;
552template <
typename FunctorType,
typename Scalar>
553LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::minimizeOptimumStorage(FVectorType &x) {
554 LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x);
555 if (status == LevenbergMarquardtSpace::ImproperInputParameters)
return status;
557 status = minimizeOptimumStorageOneStep(x);
558 }
while (status == LevenbergMarquardtSpace::Running);
562template <
typename FunctorType,
typename Scalar>
563LevenbergMarquardtSpace::Status LevenbergMarquardt<FunctorType, Scalar>::lmdif1(FunctorType &functor, FVectorType &x,
566 Index m = functor.values();
569 if (n <= 0 || m < n || tol < 0.)
return LevenbergMarquardtSpace::ImproperInputParameters;
574 lm.parameters.ftol = tol;
575 lm.parameters.xtol = tol;
576 lm.parameters.maxfev = 200 * (n + 1);
578 LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
579 if (nfev) *nfev = lm.nfev;
Performs non linear optimization over a non-linear function, using a variant of the Levenberg Marquar...
Definition LevenbergMarquardt.h:102
void resetParameters()
Definition LevenbergMarquardt.h:134
RealScalar lm_param(void)
Definition LevenbergMarquardt.h:203
Definition NumericalDiff.h:35
Namespace containing all symbols from the Eigen library.
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_inverse_op< typename Derived::Scalar >, const Derived > inverse(const Eigen::ArrayBase< Derived > &x)
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_abs_op< typename Derived::Scalar >, const Derived > abs(const Eigen::ArrayBase< Derived > &x)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index