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