28 #ifndef EIGEN_LEVENBERGMARQUARDT__H
29 #define EIGEN_LEVENBERGMARQUARDT__H
33 namespace LevenbergMarquardtSpace {
37 ImproperInputParameters = 0,
38 RelativeReductionTooSmall = 1,
39 RelativeErrorTooSmall = 2,
40 RelativeErrorAndReductionTooSmall = 3,
42 TooManyFunctionEvaluation = 5,
60 template<
typename FunctorType,
typename Scalar=
double>
65 : functor(_functor) { nfev = njev = iter = 0; fnorm = gnorm = 0.; useExternalScaling=
false; }
67 typedef DenseIndex Index;
71 : factor(Scalar(100.))
73 , ftol(internal::sqrt(NumTraits<Scalar>::epsilon()))
74 , xtol(internal::sqrt(NumTraits<Scalar>::epsilon()))
76 , epsfcn(Scalar(0.)) {}
85 typedef Matrix< Scalar, Dynamic, 1 > FVectorType;
86 typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType;
88 LevenbergMarquardtSpace::Status lmder1(
90 const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon())
93 LevenbergMarquardtSpace::Status minimize(FVectorType &x);
94 LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
95 LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
97 static LevenbergMarquardtSpace::Status lmdif1(
101 const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon())
104 LevenbergMarquardtSpace::Status lmstr1(
106 const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon())
109 LevenbergMarquardtSpace::Status minimizeOptimumStorage(FVectorType &x);
110 LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(FVectorType &x);
111 LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(FVectorType &x);
113 void resetParameters(
void) { parameters = Parameters(); }
115 Parameters parameters;
116 FVectorType fvec, qtf, diag;
118 PermutationMatrix<Dynamic,Dynamic> permutation;
123 bool useExternalScaling;
125 Scalar lm_param(
void) {
return par; }
127 FunctorType &functor;
130 FVectorType wa1, wa2, wa3, wa4;
133 Scalar temp, temp1, temp2;
136 Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
141 template<
typename FunctorType,
typename Scalar>
142 LevenbergMarquardtSpace::Status
149 m = functor.values();
152 if (n <= 0 || m < n || tol < 0.)
153 return LevenbergMarquardtSpace::ImproperInputParameters;
156 parameters.ftol = tol;
157 parameters.xtol = tol;
158 parameters.maxfev = 100*(n+1);
164 template<
typename FunctorType,
typename Scalar>
165 LevenbergMarquardtSpace::Status
166 LevenbergMarquardt<FunctorType,Scalar>::minimize(FVectorType &x)
168 LevenbergMarquardtSpace::Status status = minimizeInit(x);
169 if (status==LevenbergMarquardtSpace::ImproperInputParameters)
172 status = minimizeOneStep(x);
173 }
while (status==LevenbergMarquardtSpace::Running);
177 template<
typename FunctorType,
typename Scalar>
178 LevenbergMarquardtSpace::Status
179 LevenbergMarquardt<FunctorType,Scalar>::minimizeInit(FVectorType &x)
182 m = functor.values();
184 wa1.resize(n); wa2.resize(n); wa3.resize(n);
188 if (!useExternalScaling)
190 assert( (!useExternalScaling || diag.size()==n) ||
"When useExternalScaling is set, the caller must provide a valid 'diag'");
198 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
199 return LevenbergMarquardtSpace::ImproperInputParameters;
201 if (useExternalScaling)
202 for (Index j = 0; j < n; ++j)
204 return LevenbergMarquardtSpace::ImproperInputParameters;
209 if ( functor(x, fvec) < 0)
210 return LevenbergMarquardtSpace::UserAsked;
211 fnorm = fvec.stableNorm();
217 return LevenbergMarquardtSpace::NotStarted;
220 template<
typename FunctorType,
typename Scalar>
221 LevenbergMarquardtSpace::Status
222 LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(FVectorType &x)
227 Index df_ret = functor.df(x, fjac);
229 return LevenbergMarquardtSpace::UserAsked;
236 wa2 = fjac.colwise().blueNorm();
237 ColPivHouseholderQR<JacobianType> qrfac(fjac);
238 fjac = qrfac.matrixQR();
239 permutation = qrfac.colsPermutation();
244 if (!useExternalScaling)
245 for (Index j = 0; j < n; ++j)
246 diag[j] = (wa2[j]==0.)? 1. : wa2[j];
250 xnorm = diag.cwiseProduct(x).stableNorm();
251 delta = parameters.factor * xnorm;
253 delta = parameters.factor;
259 wa4.applyOnTheLeft(qrfac.householderQ().adjoint());
265 for (Index j = 0; j < n; ++j)
266 if (wa2[permutation.indices()[j]] != 0.)
267 gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
270 if (gnorm <= parameters.gtol)
271 return LevenbergMarquardtSpace::CosinusTooSmall;
274 if (!useExternalScaling)
275 diag = diag.cwiseMax(wa2);
280 internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1);
285 pnorm = diag.cwiseProduct(wa1).stableNorm();
289 delta = (std::min)(delta,pnorm);
292 if ( functor(wa2, wa4) < 0)
293 return LevenbergMarquardtSpace::UserAsked;
295 fnorm1 = wa4.stableNorm();
299 if (Scalar(.1) * fnorm1 < fnorm)
300 actred = 1. - internal::abs2(fnorm1 / fnorm);
304 wa3 = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() *wa1);
305 temp1 = internal::abs2(wa3.stableNorm() / fnorm);
306 temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm);
307 prered = temp1 + temp2 / Scalar(.5);
308 dirder = -(temp1 + temp2);
314 ratio = actred / prered;
317 if (ratio <= Scalar(.25)) {
321 temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
322 if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
325 delta = temp * (std::min)(delta, pnorm / Scalar(.1));
327 }
else if (!(par != 0. && ratio < Scalar(.75))) {
328 delta = pnorm / Scalar(.5);
329 par = Scalar(.5) * par;
333 if (ratio >= Scalar(1e-4)) {
336 wa2 = diag.cwiseProduct(x);
338 xnorm = wa2.stableNorm();
344 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
345 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
346 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
347 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
348 if (delta <= parameters.xtol * xnorm)
349 return LevenbergMarquardtSpace::RelativeErrorTooSmall;
352 if (nfev >= parameters.maxfev)
353 return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
354 if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
355 return LevenbergMarquardtSpace::FtolTooSmall;
356 if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
357 return LevenbergMarquardtSpace::XtolTooSmall;
358 if (gnorm <= NumTraits<Scalar>::epsilon())
359 return LevenbergMarquardtSpace::GtolTooSmall;
361 }
while (ratio < Scalar(1e-4));
363 return LevenbergMarquardtSpace::Running;
366 template<
typename FunctorType,
typename Scalar>
367 LevenbergMarquardtSpace::Status
368 LevenbergMarquardt<FunctorType,Scalar>::lmstr1(
374 m = functor.values();
377 if (n <= 0 || m < n || tol < 0.)
378 return LevenbergMarquardtSpace::ImproperInputParameters;
381 parameters.ftol = tol;
382 parameters.xtol = tol;
383 parameters.maxfev = 100*(n+1);
385 return minimizeOptimumStorage(x);
388 template<
typename FunctorType,
typename Scalar>
389 LevenbergMarquardtSpace::Status
390 LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageInit(FVectorType &x)
393 m = functor.values();
395 wa1.resize(n); wa2.resize(n); wa3.resize(n);
404 if (!useExternalScaling)
406 assert( (!useExternalScaling || diag.size()==n) ||
"When useExternalScaling is set, the caller must provide a valid 'diag'");
414 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
415 return LevenbergMarquardtSpace::ImproperInputParameters;
417 if (useExternalScaling)
418 for (Index j = 0; j < n; ++j)
420 return LevenbergMarquardtSpace::ImproperInputParameters;
425 if ( functor(x, fvec) < 0)
426 return LevenbergMarquardtSpace::UserAsked;
427 fnorm = fvec.stableNorm();
433 return LevenbergMarquardtSpace::NotStarted;
437 template<
typename FunctorType,
typename Scalar>
438 LevenbergMarquardtSpace::Status
439 LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(FVectorType &x)
453 for (i = 0; i < m; ++i) {
454 if (functor.df(x, wa3, rownb) < 0)
return LevenbergMarquardtSpace::UserAsked;
455 internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]);
463 for (j = 0; j < n; ++j) {
466 wa2[j] = fjac.col(j).head(j).stableNorm();
468 permutation.setIdentity(n);
470 wa2 = fjac.colwise().blueNorm();
473 ColPivHouseholderQR<JacobianType> qrfac(fjac);
474 fjac = qrfac.matrixQR();
475 wa1 = fjac.diagonal();
476 fjac.diagonal() = qrfac.hCoeffs();
477 permutation = qrfac.colsPermutation();
479 for(Index ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii);
481 for (j = 0; j < n; ++j) {
482 if (fjac(j,j) != 0.) {
484 for (i = j; i < n; ++i)
485 sum += fjac(i,j) * qtf[i];
486 temp = -sum / fjac(j,j);
487 for (i = j; i < n; ++i)
488 qtf[i] += fjac(i,j) * temp;
497 if (!useExternalScaling)
498 for (j = 0; j < n; ++j)
499 diag[j] = (wa2[j]==0.)? 1. : wa2[j];
503 xnorm = diag.cwiseProduct(x).stableNorm();
504 delta = parameters.factor * xnorm;
506 delta = parameters.factor;
512 for (j = 0; j < n; ++j)
513 if (wa2[permutation.indices()[j]] != 0.)
514 gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
517 if (gnorm <= parameters.gtol)
518 return LevenbergMarquardtSpace::CosinusTooSmall;
521 if (!useExternalScaling)
522 diag = diag.cwiseMax(wa2);
527 internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1);
532 pnorm = diag.cwiseProduct(wa1).stableNorm();
536 delta = (std::min)(delta,pnorm);
539 if ( functor(wa2, wa4) < 0)
540 return LevenbergMarquardtSpace::UserAsked;
542 fnorm1 = wa4.stableNorm();
546 if (Scalar(.1) * fnorm1 < fnorm)
547 actred = 1. - internal::abs2(fnorm1 / fnorm);
551 wa3 = fjac.topLeftCorner(n,n).template triangularView<Upper>() * (permutation.inverse() * wa1);
552 temp1 = internal::abs2(wa3.stableNorm() / fnorm);
553 temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm);
554 prered = temp1 + temp2 / Scalar(.5);
555 dirder = -(temp1 + temp2);
561 ratio = actred / prered;
564 if (ratio <= Scalar(.25)) {
568 temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
569 if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
572 delta = temp * (std::min)(delta, pnorm / Scalar(.1));
574 }
else if (!(par != 0. && ratio < Scalar(.75))) {
575 delta = pnorm / Scalar(.5);
576 par = Scalar(.5) * par;
580 if (ratio >= Scalar(1e-4)) {
583 wa2 = diag.cwiseProduct(x);
585 xnorm = wa2.stableNorm();
591 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
592 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
593 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
594 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
595 if (delta <= parameters.xtol * xnorm)
596 return LevenbergMarquardtSpace::RelativeErrorTooSmall;
599 if (nfev >= parameters.maxfev)
600 return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
601 if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
602 return LevenbergMarquardtSpace::FtolTooSmall;
603 if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
604 return LevenbergMarquardtSpace::XtolTooSmall;
605 if (gnorm <= NumTraits<Scalar>::epsilon())
606 return LevenbergMarquardtSpace::GtolTooSmall;
608 }
while (ratio < Scalar(1e-4));
610 return LevenbergMarquardtSpace::Running;
613 template<
typename FunctorType,
typename Scalar>
614 LevenbergMarquardtSpace::Status
615 LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage(FVectorType &x)
617 LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x);
618 if (status==LevenbergMarquardtSpace::ImproperInputParameters)
621 status = minimizeOptimumStorageOneStep(x);
622 }
while (status==LevenbergMarquardtSpace::Running);
626 template<
typename FunctorType,
typename Scalar>
627 LevenbergMarquardtSpace::Status
628 LevenbergMarquardt<FunctorType,Scalar>::lmdif1(
629 FunctorType &functor,
636 Index m = functor.values();
639 if (n <= 0 || m < n || tol < 0.)
640 return LevenbergMarquardtSpace::ImproperInputParameters;
642 NumericalDiff<FunctorType> numDiff(functor);
644 LevenbergMarquardt<NumericalDiff<FunctorType>, Scalar > lm(numDiff);
645 lm.parameters.ftol = tol;
646 lm.parameters.xtol = tol;
647 lm.parameters.maxfev = 200*(n+1);
649 LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
657 #endif // EIGEN_LEVENBERGMARQUARDT__H