13 #ifndef EIGEN_LEVENBERGMARQUARDT__H
14 #define EIGEN_LEVENBERGMARQUARDT__H
18 namespace LevenbergMarquardtSpace {
22 ImproperInputParameters = 0,
23 RelativeReductionTooSmall = 1,
24 RelativeErrorTooSmall = 2,
25 RelativeErrorAndReductionTooSmall = 3,
27 TooManyFunctionEvaluation = 5,
45 template<
typename FunctorType,
typename Scalar=
double>
50 : functor(_functor) { nfev = njev = iter = 0; fnorm = gnorm = 0.; useExternalScaling=
false; }
52 typedef DenseIndex Index;
56 : factor(Scalar(100.))
61 , epsfcn(Scalar(0.)) {}
73 LevenbergMarquardtSpace::Status lmder1(
78 LevenbergMarquardtSpace::Status minimize(
FVectorType &x);
79 LevenbergMarquardtSpace::Status minimizeInit(
FVectorType &x);
80 LevenbergMarquardtSpace::Status minimizeOneStep(
FVectorType &x);
82 static LevenbergMarquardtSpace::Status lmdif1(
89 LevenbergMarquardtSpace::Status lmstr1(
94 LevenbergMarquardtSpace::Status minimizeOptimumStorage(
FVectorType &x);
95 LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(
FVectorType &x);
96 LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(
FVectorType &x);
98 void resetParameters(
void) { parameters = Parameters(); }
100 Parameters parameters;
108 bool useExternalScaling;
110 Scalar lm_param(
void) {
return par; }
112 FunctorType &functor;
118 Scalar temp, temp1, temp2;
121 Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
126 template<
typename FunctorType,
typename Scalar>
127 LevenbergMarquardtSpace::Status
134 m = functor.values();
137 if (n <= 0 || m < n || tol < 0.)
138 return LevenbergMarquardtSpace::ImproperInputParameters;
141 parameters.ftol = tol;
142 parameters.xtol = tol;
143 parameters.maxfev = 100*(n+1);
149 template<
typename FunctorType,
typename Scalar>
150 LevenbergMarquardtSpace::Status
151 LevenbergMarquardt<FunctorType,Scalar>::minimize(FVectorType &x)
153 LevenbergMarquardtSpace::Status status = minimizeInit(x);
154 if (status==LevenbergMarquardtSpace::ImproperInputParameters)
157 status = minimizeOneStep(x);
158 }
while (status==LevenbergMarquardtSpace::Running);
162 template<
typename FunctorType,
typename Scalar>
163 LevenbergMarquardtSpace::Status
164 LevenbergMarquardt<FunctorType,Scalar>::minimizeInit(FVectorType &x)
167 m = functor.values();
169 wa1.resize(n); wa2.resize(n); wa3.resize(n);
173 if (!useExternalScaling)
175 assert( (!useExternalScaling || diag.size()==n) ||
"When useExternalScaling is set, the caller must provide a valid 'diag'");
183 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
184 return LevenbergMarquardtSpace::ImproperInputParameters;
186 if (useExternalScaling)
187 for (Index j = 0; j < n; ++j)
189 return LevenbergMarquardtSpace::ImproperInputParameters;
194 if ( functor(x, fvec) < 0)
195 return LevenbergMarquardtSpace::UserAsked;
196 fnorm = fvec.stableNorm();
202 return LevenbergMarquardtSpace::NotStarted;
205 template<
typename FunctorType,
typename Scalar>
206 LevenbergMarquardtSpace::Status
207 LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(FVectorType &x)
212 Index df_ret = functor.df(x, fjac);
214 return LevenbergMarquardtSpace::UserAsked;
221 wa2 = fjac.colwise().blueNorm();
222 ColPivHouseholderQR<JacobianType> qrfac(fjac);
223 fjac = qrfac.matrixQR();
224 permutation = qrfac.colsPermutation();
229 if (!useExternalScaling)
230 for (Index j = 0; j < n; ++j)
231 diag[j] = (wa2[j]==0.)? 1. : wa2[j];
235 xnorm = diag.cwiseProduct(x).stableNorm();
236 delta = parameters.factor * xnorm;
238 delta = parameters.factor;
244 wa4.applyOnTheLeft(qrfac.householderQ().adjoint());
250 for (Index j = 0; j < n; ++j)
251 if (wa2[permutation.indices()[j]] != 0.)
252 gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
255 if (gnorm <= parameters.gtol)
256 return LevenbergMarquardtSpace::CosinusTooSmall;
259 if (!useExternalScaling)
260 diag = diag.cwiseMax(wa2);
265 internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1);
270 pnorm = diag.cwiseProduct(wa1).stableNorm();
274 delta = (std::min)(delta,pnorm);
277 if ( functor(wa2, wa4) < 0)
278 return LevenbergMarquardtSpace::UserAsked;
280 fnorm1 = wa4.stableNorm();
284 if (Scalar(.1) * fnorm1 < fnorm)
285 actred = 1. - internal::abs2(fnorm1 / fnorm);
289 wa3 = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() *wa1);
290 temp1 = internal::abs2(wa3.stableNorm() / fnorm);
291 temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm);
292 prered = temp1 + temp2 / Scalar(.5);
293 dirder = -(temp1 + temp2);
299 ratio = actred / prered;
302 if (ratio <= Scalar(.25)) {
306 temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
307 if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
310 delta = temp * (std::min)(delta, pnorm / Scalar(.1));
312 }
else if (!(par != 0. && ratio < Scalar(.75))) {
313 delta = pnorm / Scalar(.5);
314 par = Scalar(.5) * par;
318 if (ratio >= Scalar(1e-4)) {
321 wa2 = diag.cwiseProduct(x);
323 xnorm = wa2.stableNorm();
329 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
330 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
331 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
332 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
333 if (delta <= parameters.xtol * xnorm)
334 return LevenbergMarquardtSpace::RelativeErrorTooSmall;
337 if (nfev >= parameters.maxfev)
338 return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
339 if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
340 return LevenbergMarquardtSpace::FtolTooSmall;
341 if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
342 return LevenbergMarquardtSpace::XtolTooSmall;
343 if (gnorm <= NumTraits<Scalar>::epsilon())
344 return LevenbergMarquardtSpace::GtolTooSmall;
346 }
while (ratio < Scalar(1e-4));
348 return LevenbergMarquardtSpace::Running;
351 template<
typename FunctorType,
typename Scalar>
352 LevenbergMarquardtSpace::Status
353 LevenbergMarquardt<FunctorType,Scalar>::lmstr1(
359 m = functor.values();
362 if (n <= 0 || m < n || tol < 0.)
363 return LevenbergMarquardtSpace::ImproperInputParameters;
366 parameters.ftol = tol;
367 parameters.xtol = tol;
368 parameters.maxfev = 100*(n+1);
370 return minimizeOptimumStorage(x);
373 template<
typename FunctorType,
typename Scalar>
374 LevenbergMarquardtSpace::Status
375 LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageInit(FVectorType &x)
378 m = functor.values();
380 wa1.resize(n); wa2.resize(n); wa3.resize(n);
389 if (!useExternalScaling)
391 assert( (!useExternalScaling || diag.size()==n) ||
"When useExternalScaling is set, the caller must provide a valid 'diag'");
399 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
400 return LevenbergMarquardtSpace::ImproperInputParameters;
402 if (useExternalScaling)
403 for (Index j = 0; j < n; ++j)
405 return LevenbergMarquardtSpace::ImproperInputParameters;
410 if ( functor(x, fvec) < 0)
411 return LevenbergMarquardtSpace::UserAsked;
412 fnorm = fvec.stableNorm();
418 return LevenbergMarquardtSpace::NotStarted;
422 template<
typename FunctorType,
typename Scalar>
423 LevenbergMarquardtSpace::Status
424 LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(FVectorType &x)
438 for (i = 0; i < m; ++i) {
439 if (functor.df(x, wa3, rownb) < 0)
return LevenbergMarquardtSpace::UserAsked;
440 internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]);
448 for (j = 0; j < n; ++j) {
451 wa2[j] = fjac.col(j).head(j).stableNorm();
453 permutation.setIdentity(n);
455 wa2 = fjac.colwise().blueNorm();
458 ColPivHouseholderQR<JacobianType> qrfac(fjac);
459 fjac = qrfac.matrixQR();
460 wa1 = fjac.diagonal();
461 fjac.diagonal() = qrfac.hCoeffs();
462 permutation = qrfac.colsPermutation();
464 for(Index ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii);
466 for (j = 0; j < n; ++j) {
467 if (fjac(j,j) != 0.) {
469 for (i = j; i < n; ++i)
470 sum += fjac(i,j) * qtf[i];
471 temp = -sum / fjac(j,j);
472 for (i = j; i < n; ++i)
473 qtf[i] += fjac(i,j) * temp;
482 if (!useExternalScaling)
483 for (j = 0; j < n; ++j)
484 diag[j] = (wa2[j]==0.)? 1. : wa2[j];
488 xnorm = diag.cwiseProduct(x).stableNorm();
489 delta = parameters.factor * xnorm;
491 delta = parameters.factor;
497 for (j = 0; j < n; ++j)
498 if (wa2[permutation.indices()[j]] != 0.)
499 gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
502 if (gnorm <= parameters.gtol)
503 return LevenbergMarquardtSpace::CosinusTooSmall;
506 if (!useExternalScaling)
507 diag = diag.cwiseMax(wa2);
512 internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1);
517 pnorm = diag.cwiseProduct(wa1).stableNorm();
521 delta = (std::min)(delta,pnorm);
524 if ( functor(wa2, wa4) < 0)
525 return LevenbergMarquardtSpace::UserAsked;
527 fnorm1 = wa4.stableNorm();
531 if (Scalar(.1) * fnorm1 < fnorm)
532 actred = 1. - internal::abs2(fnorm1 / fnorm);
536 wa3 = fjac.topLeftCorner(n,n).template triangularView<Upper>() * (permutation.inverse() * wa1);
537 temp1 = internal::abs2(wa3.stableNorm() / fnorm);
538 temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm);
539 prered = temp1 + temp2 / Scalar(.5);
540 dirder = -(temp1 + temp2);
546 ratio = actred / prered;
549 if (ratio <= Scalar(.25)) {
553 temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
554 if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
557 delta = temp * (std::min)(delta, pnorm / Scalar(.1));
559 }
else if (!(par != 0. && ratio < Scalar(.75))) {
560 delta = pnorm / Scalar(.5);
561 par = Scalar(.5) * par;
565 if (ratio >= Scalar(1e-4)) {
568 wa2 = diag.cwiseProduct(x);
570 xnorm = wa2.stableNorm();
576 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
577 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
578 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
579 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
580 if (delta <= parameters.xtol * xnorm)
581 return LevenbergMarquardtSpace::RelativeErrorTooSmall;
584 if (nfev >= parameters.maxfev)
585 return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
586 if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
587 return LevenbergMarquardtSpace::FtolTooSmall;
588 if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
589 return LevenbergMarquardtSpace::XtolTooSmall;
590 if (gnorm <= NumTraits<Scalar>::epsilon())
591 return LevenbergMarquardtSpace::GtolTooSmall;
593 }
while (ratio < Scalar(1e-4));
595 return LevenbergMarquardtSpace::Running;
598 template<
typename FunctorType,
typename Scalar>
599 LevenbergMarquardtSpace::Status
600 LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage(FVectorType &x)
602 LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x);
603 if (status==LevenbergMarquardtSpace::ImproperInputParameters)
606 status = minimizeOptimumStorageOneStep(x);
607 }
while (status==LevenbergMarquardtSpace::Running);
611 template<
typename FunctorType,
typename Scalar>
612 LevenbergMarquardtSpace::Status
613 LevenbergMarquardt<FunctorType,Scalar>::lmdif1(
614 FunctorType &functor,
621 Index m = functor.values();
624 if (n <= 0 || m < n || tol < 0.)
625 return LevenbergMarquardtSpace::ImproperInputParameters;
627 NumericalDiff<FunctorType> numDiff(functor);
629 LevenbergMarquardt<NumericalDiff<FunctorType>, Scalar > lm(numDiff);
630 lm.parameters.ftol = tol;
631 lm.parameters.xtol = tol;
632 lm.parameters.maxfev = 200*(n+1);
634 LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
642 #endif // EIGEN_LEVENBERGMARQUARDT__H