00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef EIGEN_LEVENBERGMARQUARDT__H
00029 #define EIGEN_LEVENBERGMARQUARDT__H
00030
00031 namespace Eigen {
00032
00033 namespace LevenbergMarquardtSpace {
00034 enum Status {
00035 NotStarted = -2,
00036 Running = -1,
00037 ImproperInputParameters = 0,
00038 RelativeReductionTooSmall = 1,
00039 RelativeErrorTooSmall = 2,
00040 RelativeErrorAndReductionTooSmall = 3,
00041 CosinusTooSmall = 4,
00042 TooManyFunctionEvaluation = 5,
00043 FtolTooSmall = 6,
00044 XtolTooSmall = 7,
00045 GtolTooSmall = 8,
00046 UserAsked = 9
00047 };
00048 }
00049
00050
00051
00060 template<typename FunctorType, typename Scalar=double>
00061 class LevenbergMarquardt
00062 {
00063 public:
00064 LevenbergMarquardt(FunctorType &_functor)
00065 : functor(_functor) { nfev = njev = iter = 0; fnorm = gnorm = 0.; useExternalScaling=false; }
00066
00067 typedef DenseIndex Index;
00068
00069 struct Parameters {
00070 Parameters()
00071 : factor(Scalar(100.))
00072 , maxfev(400)
00073 , ftol(internal::sqrt(NumTraits<Scalar>::epsilon()))
00074 , xtol(internal::sqrt(NumTraits<Scalar>::epsilon()))
00075 , gtol(Scalar(0.))
00076 , epsfcn(Scalar(0.)) {}
00077 Scalar factor;
00078 Index maxfev;
00079 Scalar ftol;
00080 Scalar xtol;
00081 Scalar gtol;
00082 Scalar epsfcn;
00083 };
00084
00085 typedef Matrix< Scalar, Dynamic, 1 > FVectorType;
00086 typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType;
00087
00088 LevenbergMarquardtSpace::Status lmder1(
00089 FVectorType &x,
00090 const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon())
00091 );
00092
00093 LevenbergMarquardtSpace::Status minimize(FVectorType &x);
00094 LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
00095 LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
00096
00097 static LevenbergMarquardtSpace::Status lmdif1(
00098 FunctorType &functor,
00099 FVectorType &x,
00100 Index *nfev,
00101 const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon())
00102 );
00103
00104 LevenbergMarquardtSpace::Status lmstr1(
00105 FVectorType &x,
00106 const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon())
00107 );
00108
00109 LevenbergMarquardtSpace::Status minimizeOptimumStorage(FVectorType &x);
00110 LevenbergMarquardtSpace::Status minimizeOptimumStorageInit(FVectorType &x);
00111 LevenbergMarquardtSpace::Status minimizeOptimumStorageOneStep(FVectorType &x);
00112
00113 void resetParameters(void) { parameters = Parameters(); }
00114
00115 Parameters parameters;
00116 FVectorType fvec, qtf, diag;
00117 JacobianType fjac;
00118 PermutationMatrix<Dynamic,Dynamic> permutation;
00119 Index nfev;
00120 Index njev;
00121 Index iter;
00122 Scalar fnorm, gnorm;
00123 bool useExternalScaling;
00124
00125 Scalar lm_param(void) { return par; }
00126 private:
00127 FunctorType &functor;
00128 Index n;
00129 Index m;
00130 FVectorType wa1, wa2, wa3, wa4;
00131
00132 Scalar par, sum;
00133 Scalar temp, temp1, temp2;
00134 Scalar delta;
00135 Scalar ratio;
00136 Scalar pnorm, xnorm, fnorm1, actred, dirder, prered;
00137
00138 LevenbergMarquardt& operator=(const LevenbergMarquardt&);
00139 };
00140
00141 template<typename FunctorType, typename Scalar>
00142 LevenbergMarquardtSpace::Status
00143 LevenbergMarquardt<FunctorType,Scalar>::lmder1(
00144 FVectorType &x,
00145 const Scalar tol
00146 )
00147 {
00148 n = x.size();
00149 m = functor.values();
00150
00151
00152 if (n <= 0 || m < n || tol < 0.)
00153 return LevenbergMarquardtSpace::ImproperInputParameters;
00154
00155 resetParameters();
00156 parameters.ftol = tol;
00157 parameters.xtol = tol;
00158 parameters.maxfev = 100*(n+1);
00159
00160 return minimize(x);
00161 }
00162
00163
00164 template<typename FunctorType, typename Scalar>
00165 LevenbergMarquardtSpace::Status
00166 LevenbergMarquardt<FunctorType,Scalar>::minimize(FVectorType &x)
00167 {
00168 LevenbergMarquardtSpace::Status status = minimizeInit(x);
00169 if (status==LevenbergMarquardtSpace::ImproperInputParameters)
00170 return status;
00171 do {
00172 status = minimizeOneStep(x);
00173 } while (status==LevenbergMarquardtSpace::Running);
00174 return status;
00175 }
00176
00177 template<typename FunctorType, typename Scalar>
00178 LevenbergMarquardtSpace::Status
00179 LevenbergMarquardt<FunctorType,Scalar>::minimizeInit(FVectorType &x)
00180 {
00181 n = x.size();
00182 m = functor.values();
00183
00184 wa1.resize(n); wa2.resize(n); wa3.resize(n);
00185 wa4.resize(m);
00186 fvec.resize(m);
00187 fjac.resize(m, n);
00188 if (!useExternalScaling)
00189 diag.resize(n);
00190 assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'");
00191 qtf.resize(n);
00192
00193
00194 nfev = 0;
00195 njev = 0;
00196
00197
00198 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
00199 return LevenbergMarquardtSpace::ImproperInputParameters;
00200
00201 if (useExternalScaling)
00202 for (Index j = 0; j < n; ++j)
00203 if (diag[j] <= 0.)
00204 return LevenbergMarquardtSpace::ImproperInputParameters;
00205
00206
00207
00208 nfev = 1;
00209 if ( functor(x, fvec) < 0)
00210 return LevenbergMarquardtSpace::UserAsked;
00211 fnorm = fvec.stableNorm();
00212
00213
00214 par = 0.;
00215 iter = 1;
00216
00217 return LevenbergMarquardtSpace::NotStarted;
00218 }
00219
00220 template<typename FunctorType, typename Scalar>
00221 LevenbergMarquardtSpace::Status
00222 LevenbergMarquardt<FunctorType,Scalar>::minimizeOneStep(FVectorType &x)
00223 {
00224 assert(x.size()==n);
00225
00226
00227 Index df_ret = functor.df(x, fjac);
00228 if (df_ret<0)
00229 return LevenbergMarquardtSpace::UserAsked;
00230 if (df_ret>0)
00231
00232 nfev += df_ret;
00233 else njev++;
00234
00235
00236 wa2 = fjac.colwise().blueNorm();
00237 ColPivHouseholderQR<JacobianType> qrfac(fjac);
00238 fjac = qrfac.matrixQR();
00239 permutation = qrfac.colsPermutation();
00240
00241
00242
00243 if (iter == 1) {
00244 if (!useExternalScaling)
00245 for (Index j = 0; j < n; ++j)
00246 diag[j] = (wa2[j]==0.)? 1. : wa2[j];
00247
00248
00249
00250 xnorm = diag.cwiseProduct(x).stableNorm();
00251 delta = parameters.factor * xnorm;
00252 if (delta == 0.)
00253 delta = parameters.factor;
00254 }
00255
00256
00257
00258 wa4 = fvec;
00259 wa4.applyOnTheLeft(qrfac.householderQ().adjoint());
00260 qtf = wa4.head(n);
00261
00262
00263 gnorm = 0.;
00264 if (fnorm != 0.)
00265 for (Index j = 0; j < n; ++j)
00266 if (wa2[permutation.indices()[j]] != 0.)
00267 gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
00268
00269
00270 if (gnorm <= parameters.gtol)
00271 return LevenbergMarquardtSpace::CosinusTooSmall;
00272
00273
00274 if (!useExternalScaling)
00275 diag = diag.cwiseMax(wa2);
00276
00277 do {
00278
00279
00280 internal::lmpar2<Scalar>(qrfac, diag, qtf, delta, par, wa1);
00281
00282
00283 wa1 = -wa1;
00284 wa2 = x + wa1;
00285 pnorm = diag.cwiseProduct(wa1).stableNorm();
00286
00287
00288 if (iter == 1)
00289 delta = (std::min)(delta,pnorm);
00290
00291
00292 if ( functor(wa2, wa4) < 0)
00293 return LevenbergMarquardtSpace::UserAsked;
00294 ++nfev;
00295 fnorm1 = wa4.stableNorm();
00296
00297
00298 actred = -1.;
00299 if (Scalar(.1) * fnorm1 < fnorm)
00300 actred = 1. - internal::abs2(fnorm1 / fnorm);
00301
00302
00303
00304 wa3 = fjac.template triangularView<Upper>() * (qrfac.colsPermutation().inverse() *wa1);
00305 temp1 = internal::abs2(wa3.stableNorm() / fnorm);
00306 temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm);
00307 prered = temp1 + temp2 / Scalar(.5);
00308 dirder = -(temp1 + temp2);
00309
00310
00311
00312 ratio = 0.;
00313 if (prered != 0.)
00314 ratio = actred / prered;
00315
00316
00317 if (ratio <= Scalar(.25)) {
00318 if (actred >= 0.)
00319 temp = Scalar(.5);
00320 if (actred < 0.)
00321 temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
00322 if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
00323 temp = Scalar(.1);
00324
00325 delta = temp * (std::min)(delta, pnorm / Scalar(.1));
00326 par /= temp;
00327 } else if (!(par != 0. && ratio < Scalar(.75))) {
00328 delta = pnorm / Scalar(.5);
00329 par = Scalar(.5) * par;
00330 }
00331
00332
00333 if (ratio >= Scalar(1e-4)) {
00334
00335 x = wa2;
00336 wa2 = diag.cwiseProduct(x);
00337 fvec = wa4;
00338 xnorm = wa2.stableNorm();
00339 fnorm = fnorm1;
00340 ++iter;
00341 }
00342
00343
00344 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
00345 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
00346 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
00347 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
00348 if (delta <= parameters.xtol * xnorm)
00349 return LevenbergMarquardtSpace::RelativeErrorTooSmall;
00350
00351
00352 if (nfev >= parameters.maxfev)
00353 return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
00354 if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
00355 return LevenbergMarquardtSpace::FtolTooSmall;
00356 if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
00357 return LevenbergMarquardtSpace::XtolTooSmall;
00358 if (gnorm <= NumTraits<Scalar>::epsilon())
00359 return LevenbergMarquardtSpace::GtolTooSmall;
00360
00361 } while (ratio < Scalar(1e-4));
00362
00363 return LevenbergMarquardtSpace::Running;
00364 }
00365
00366 template<typename FunctorType, typename Scalar>
00367 LevenbergMarquardtSpace::Status
00368 LevenbergMarquardt<FunctorType,Scalar>::lmstr1(
00369 FVectorType &x,
00370 const Scalar tol
00371 )
00372 {
00373 n = x.size();
00374 m = functor.values();
00375
00376
00377 if (n <= 0 || m < n || tol < 0.)
00378 return LevenbergMarquardtSpace::ImproperInputParameters;
00379
00380 resetParameters();
00381 parameters.ftol = tol;
00382 parameters.xtol = tol;
00383 parameters.maxfev = 100*(n+1);
00384
00385 return minimizeOptimumStorage(x);
00386 }
00387
00388 template<typename FunctorType, typename Scalar>
00389 LevenbergMarquardtSpace::Status
00390 LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageInit(FVectorType &x)
00391 {
00392 n = x.size();
00393 m = functor.values();
00394
00395 wa1.resize(n); wa2.resize(n); wa3.resize(n);
00396 wa4.resize(m);
00397 fvec.resize(m);
00398
00399
00400
00401
00402
00403 fjac.resize(n, n);
00404 if (!useExternalScaling)
00405 diag.resize(n);
00406 assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'");
00407 qtf.resize(n);
00408
00409
00410 nfev = 0;
00411 njev = 0;
00412
00413
00414 if (n <= 0 || m < n || parameters.ftol < 0. || parameters.xtol < 0. || parameters.gtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
00415 return LevenbergMarquardtSpace::ImproperInputParameters;
00416
00417 if (useExternalScaling)
00418 for (Index j = 0; j < n; ++j)
00419 if (diag[j] <= 0.)
00420 return LevenbergMarquardtSpace::ImproperInputParameters;
00421
00422
00423
00424 nfev = 1;
00425 if ( functor(x, fvec) < 0)
00426 return LevenbergMarquardtSpace::UserAsked;
00427 fnorm = fvec.stableNorm();
00428
00429
00430 par = 0.;
00431 iter = 1;
00432
00433 return LevenbergMarquardtSpace::NotStarted;
00434 }
00435
00436
00437 template<typename FunctorType, typename Scalar>
00438 LevenbergMarquardtSpace::Status
00439 LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorageOneStep(FVectorType &x)
00440 {
00441 assert(x.size()==n);
00442
00443 Index i, j;
00444 bool sing;
00445
00446
00447
00448
00449
00450 qtf.fill(0.);
00451 fjac.fill(0.);
00452 Index rownb = 2;
00453 for (i = 0; i < m; ++i) {
00454 if (functor.df(x, wa3, rownb) < 0) return LevenbergMarquardtSpace::UserAsked;
00455 internal::rwupdt<Scalar>(fjac, wa3, qtf, fvec[i]);
00456 ++rownb;
00457 }
00458 ++njev;
00459
00460
00461
00462 sing = false;
00463 for (j = 0; j < n; ++j) {
00464 if (fjac(j,j) == 0.)
00465 sing = true;
00466 wa2[j] = fjac.col(j).head(j).stableNorm();
00467 }
00468 permutation.setIdentity(n);
00469 if (sing) {
00470 wa2 = fjac.colwise().blueNorm();
00471
00472
00473 ColPivHouseholderQR<JacobianType> qrfac(fjac);
00474 fjac = qrfac.matrixQR();
00475 wa1 = fjac.diagonal();
00476 fjac.diagonal() = qrfac.hCoeffs();
00477 permutation = qrfac.colsPermutation();
00478
00479 for(Index ii=0; ii< fjac.cols(); ii++) fjac.col(ii).segment(ii+1, fjac.rows()-ii-1) *= fjac(ii,ii);
00480
00481 for (j = 0; j < n; ++j) {
00482 if (fjac(j,j) != 0.) {
00483 sum = 0.;
00484 for (i = j; i < n; ++i)
00485 sum += fjac(i,j) * qtf[i];
00486 temp = -sum / fjac(j,j);
00487 for (i = j; i < n; ++i)
00488 qtf[i] += fjac(i,j) * temp;
00489 }
00490 fjac(j,j) = wa1[j];
00491 }
00492 }
00493
00494
00495
00496 if (iter == 1) {
00497 if (!useExternalScaling)
00498 for (j = 0; j < n; ++j)
00499 diag[j] = (wa2[j]==0.)? 1. : wa2[j];
00500
00501
00502
00503 xnorm = diag.cwiseProduct(x).stableNorm();
00504 delta = parameters.factor * xnorm;
00505 if (delta == 0.)
00506 delta = parameters.factor;
00507 }
00508
00509
00510 gnorm = 0.;
00511 if (fnorm != 0.)
00512 for (j = 0; j < n; ++j)
00513 if (wa2[permutation.indices()[j]] != 0.)
00514 gnorm = (std::max)(gnorm, internal::abs( fjac.col(j).head(j+1).dot(qtf.head(j+1)/fnorm) / wa2[permutation.indices()[j]]));
00515
00516
00517 if (gnorm <= parameters.gtol)
00518 return LevenbergMarquardtSpace::CosinusTooSmall;
00519
00520
00521 if (!useExternalScaling)
00522 diag = diag.cwiseMax(wa2);
00523
00524 do {
00525
00526
00527 internal::lmpar<Scalar>(fjac, permutation.indices(), diag, qtf, delta, par, wa1);
00528
00529
00530 wa1 = -wa1;
00531 wa2 = x + wa1;
00532 pnorm = diag.cwiseProduct(wa1).stableNorm();
00533
00534
00535 if (iter == 1)
00536 delta = (std::min)(delta,pnorm);
00537
00538
00539 if ( functor(wa2, wa4) < 0)
00540 return LevenbergMarquardtSpace::UserAsked;
00541 ++nfev;
00542 fnorm1 = wa4.stableNorm();
00543
00544
00545 actred = -1.;
00546 if (Scalar(.1) * fnorm1 < fnorm)
00547 actred = 1. - internal::abs2(fnorm1 / fnorm);
00548
00549
00550
00551 wa3 = fjac.topLeftCorner(n,n).template triangularView<Upper>() * (permutation.inverse() * wa1);
00552 temp1 = internal::abs2(wa3.stableNorm() / fnorm);
00553 temp2 = internal::abs2(internal::sqrt(par) * pnorm / fnorm);
00554 prered = temp1 + temp2 / Scalar(.5);
00555 dirder = -(temp1 + temp2);
00556
00557
00558
00559 ratio = 0.;
00560 if (prered != 0.)
00561 ratio = actred / prered;
00562
00563
00564 if (ratio <= Scalar(.25)) {
00565 if (actred >= 0.)
00566 temp = Scalar(.5);
00567 if (actred < 0.)
00568 temp = Scalar(.5) * dirder / (dirder + Scalar(.5) * actred);
00569 if (Scalar(.1) * fnorm1 >= fnorm || temp < Scalar(.1))
00570 temp = Scalar(.1);
00571
00572 delta = temp * (std::min)(delta, pnorm / Scalar(.1));
00573 par /= temp;
00574 } else if (!(par != 0. && ratio < Scalar(.75))) {
00575 delta = pnorm / Scalar(.5);
00576 par = Scalar(.5) * par;
00577 }
00578
00579
00580 if (ratio >= Scalar(1e-4)) {
00581
00582 x = wa2;
00583 wa2 = diag.cwiseProduct(x);
00584 fvec = wa4;
00585 xnorm = wa2.stableNorm();
00586 fnorm = fnorm1;
00587 ++iter;
00588 }
00589
00590
00591 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1. && delta <= parameters.xtol * xnorm)
00592 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
00593 if (internal::abs(actred) <= parameters.ftol && prered <= parameters.ftol && Scalar(.5) * ratio <= 1.)
00594 return LevenbergMarquardtSpace::RelativeReductionTooSmall;
00595 if (delta <= parameters.xtol * xnorm)
00596 return LevenbergMarquardtSpace::RelativeErrorTooSmall;
00597
00598
00599 if (nfev >= parameters.maxfev)
00600 return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
00601 if (internal::abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
00602 return LevenbergMarquardtSpace::FtolTooSmall;
00603 if (delta <= NumTraits<Scalar>::epsilon() * xnorm)
00604 return LevenbergMarquardtSpace::XtolTooSmall;
00605 if (gnorm <= NumTraits<Scalar>::epsilon())
00606 return LevenbergMarquardtSpace::GtolTooSmall;
00607
00608 } while (ratio < Scalar(1e-4));
00609
00610 return LevenbergMarquardtSpace::Running;
00611 }
00612
00613 template<typename FunctorType, typename Scalar>
00614 LevenbergMarquardtSpace::Status
00615 LevenbergMarquardt<FunctorType,Scalar>::minimizeOptimumStorage(FVectorType &x)
00616 {
00617 LevenbergMarquardtSpace::Status status = minimizeOptimumStorageInit(x);
00618 if (status==LevenbergMarquardtSpace::ImproperInputParameters)
00619 return status;
00620 do {
00621 status = minimizeOptimumStorageOneStep(x);
00622 } while (status==LevenbergMarquardtSpace::Running);
00623 return status;
00624 }
00625
00626 template<typename FunctorType, typename Scalar>
00627 LevenbergMarquardtSpace::Status
00628 LevenbergMarquardt<FunctorType,Scalar>::lmdif1(
00629 FunctorType &functor,
00630 FVectorType &x,
00631 Index *nfev,
00632 const Scalar tol
00633 )
00634 {
00635 Index n = x.size();
00636 Index m = functor.values();
00637
00638
00639 if (n <= 0 || m < n || tol < 0.)
00640 return LevenbergMarquardtSpace::ImproperInputParameters;
00641
00642 NumericalDiff<FunctorType> numDiff(functor);
00643
00644 LevenbergMarquardt<NumericalDiff<FunctorType>, Scalar > lm(numDiff);
00645 lm.parameters.ftol = tol;
00646 lm.parameters.xtol = tol;
00647 lm.parameters.maxfev = 200*(n+1);
00648
00649 LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
00650 if (nfev)
00651 * nfev = lm.nfev;
00652 return info;
00653 }
00654
00655 }
00656
00657 #endif // EIGEN_LEVENBERGMARQUARDT__H
00658
00659