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