File indexing completed on 2025-01-18 09:57:05
0001
0002
0003
0004
0005
0006
0007
0008
0009
0010
0011
0012
0013 #ifndef EIGEN_HYBRIDNONLINEARSOLVER_H
0014 #define EIGEN_HYBRIDNONLINEARSOLVER_H
0015
0016 namespace Eigen {
0017
0018 namespace HybridNonLinearSolverSpace {
0019 enum Status {
0020 Running = -1,
0021 ImproperInputParameters = 0,
0022 RelativeErrorTooSmall = 1,
0023 TooManyFunctionEvaluation = 2,
0024 TolTooSmall = 3,
0025 NotMakingProgressJacobian = 4,
0026 NotMakingProgressIterations = 5,
0027 UserAsked = 6
0028 };
0029 }
0030
0031
0032
0033
0034
0035
0036
0037
0038
0039
0040
0041
0042 template<typename FunctorType, typename Scalar=double>
0043 class HybridNonLinearSolver
0044 {
0045 public:
0046 typedef DenseIndex Index;
0047
0048 HybridNonLinearSolver(FunctorType &_functor)
0049 : functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; useExternalScaling=false;}
0050
0051 struct Parameters {
0052 Parameters()
0053 : factor(Scalar(100.))
0054 , maxfev(1000)
0055 , xtol(numext::sqrt(NumTraits<Scalar>::epsilon()))
0056 , nb_of_subdiagonals(-1)
0057 , nb_of_superdiagonals(-1)
0058 , epsfcn(Scalar(0.)) {}
0059 Scalar factor;
0060 Index maxfev;
0061 Scalar xtol;
0062 Index nb_of_subdiagonals;
0063 Index nb_of_superdiagonals;
0064 Scalar epsfcn;
0065 };
0066 typedef Matrix< Scalar, Dynamic, 1 > FVectorType;
0067 typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType;
0068
0069 typedef Matrix< Scalar, Dynamic, Dynamic > UpperTriangularType;
0070
0071 HybridNonLinearSolverSpace::Status hybrj1(
0072 FVectorType &x,
0073 const Scalar tol = numext::sqrt(NumTraits<Scalar>::epsilon())
0074 );
0075
0076 HybridNonLinearSolverSpace::Status solveInit(FVectorType &x);
0077 HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x);
0078 HybridNonLinearSolverSpace::Status solve(FVectorType &x);
0079
0080 HybridNonLinearSolverSpace::Status hybrd1(
0081 FVectorType &x,
0082 const Scalar tol = numext::sqrt(NumTraits<Scalar>::epsilon())
0083 );
0084
0085 HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x);
0086 HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x);
0087 HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x);
0088
0089 void resetParameters(void) { parameters = Parameters(); }
0090 Parameters parameters;
0091 FVectorType fvec, qtf, diag;
0092 JacobianType fjac;
0093 UpperTriangularType R;
0094 Index nfev;
0095 Index njev;
0096 Index iter;
0097 Scalar fnorm;
0098 bool useExternalScaling;
0099 private:
0100 FunctorType &functor;
0101 Index n;
0102 Scalar sum;
0103 bool sing;
0104 Scalar temp;
0105 Scalar delta;
0106 bool jeval;
0107 Index ncsuc;
0108 Scalar ratio;
0109 Scalar pnorm, xnorm, fnorm1;
0110 Index nslow1, nslow2;
0111 Index ncfail;
0112 Scalar actred, prered;
0113 FVectorType wa1, wa2, wa3, wa4;
0114
0115 HybridNonLinearSolver& operator=(const HybridNonLinearSolver&);
0116 };
0117
0118
0119
0120 template<typename FunctorType, typename Scalar>
0121 HybridNonLinearSolverSpace::Status
0122 HybridNonLinearSolver<FunctorType,Scalar>::hybrj1(
0123 FVectorType &x,
0124 const Scalar tol
0125 )
0126 {
0127 n = x.size();
0128
0129
0130 if (n <= 0 || tol < 0.)
0131 return HybridNonLinearSolverSpace::ImproperInputParameters;
0132
0133 resetParameters();
0134 parameters.maxfev = 100*(n+1);
0135 parameters.xtol = tol;
0136 diag.setConstant(n, 1.);
0137 useExternalScaling = true;
0138 return solve(x);
0139 }
0140
0141 template<typename FunctorType, typename Scalar>
0142 HybridNonLinearSolverSpace::Status
0143 HybridNonLinearSolver<FunctorType,Scalar>::solveInit(FVectorType &x)
0144 {
0145 n = x.size();
0146
0147 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
0148 fvec.resize(n);
0149 qtf.resize(n);
0150 fjac.resize(n, n);
0151 if (!useExternalScaling)
0152 diag.resize(n);
0153 eigen_assert( (!useExternalScaling || diag.size()==n) && "When useExternalScaling is set, the caller must provide a valid 'diag'");
0154
0155
0156 nfev = 0;
0157 njev = 0;
0158
0159
0160 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. )
0161 return HybridNonLinearSolverSpace::ImproperInputParameters;
0162 if (useExternalScaling)
0163 for (Index j = 0; j < n; ++j)
0164 if (diag[j] <= 0.)
0165 return HybridNonLinearSolverSpace::ImproperInputParameters;
0166
0167
0168
0169 nfev = 1;
0170 if ( functor(x, fvec) < 0)
0171 return HybridNonLinearSolverSpace::UserAsked;
0172 fnorm = fvec.stableNorm();
0173
0174
0175 iter = 1;
0176 ncsuc = 0;
0177 ncfail = 0;
0178 nslow1 = 0;
0179 nslow2 = 0;
0180
0181 return HybridNonLinearSolverSpace::Running;
0182 }
0183
0184 template<typename FunctorType, typename Scalar>
0185 HybridNonLinearSolverSpace::Status
0186 HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(FVectorType &x)
0187 {
0188 using std::abs;
0189
0190 eigen_assert(x.size()==n);
0191
0192 Index j;
0193 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
0194
0195 jeval = true;
0196
0197
0198 if ( functor.df(x, fjac) < 0)
0199 return HybridNonLinearSolverSpace::UserAsked;
0200 ++njev;
0201
0202 wa2 = fjac.colwise().blueNorm();
0203
0204
0205
0206 if (iter == 1) {
0207 if (!useExternalScaling)
0208 for (j = 0; j < n; ++j)
0209 diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
0210
0211
0212
0213 xnorm = diag.cwiseProduct(x).stableNorm();
0214 delta = parameters.factor * xnorm;
0215 if (delta == 0.)
0216 delta = parameters.factor;
0217 }
0218
0219
0220 HouseholderQR<JacobianType> qrfac(fjac);
0221
0222
0223 R = qrfac.matrixQR();
0224
0225
0226 fjac = qrfac.householderQ();
0227
0228
0229 qtf = fjac.transpose() * fvec;
0230
0231
0232 if (!useExternalScaling)
0233 diag = diag.cwiseMax(wa2);
0234
0235 while (true) {
0236
0237 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
0238
0239
0240 wa1 = -wa1;
0241 wa2 = x + wa1;
0242 pnorm = diag.cwiseProduct(wa1).stableNorm();
0243
0244
0245 if (iter == 1)
0246 delta = (std::min)(delta,pnorm);
0247
0248
0249 if ( functor(wa2, wa4) < 0)
0250 return HybridNonLinearSolverSpace::UserAsked;
0251 ++nfev;
0252 fnorm1 = wa4.stableNorm();
0253
0254
0255 actred = -1.;
0256 if (fnorm1 < fnorm)
0257 actred = 1. - numext::abs2(fnorm1 / fnorm);
0258
0259
0260 wa3 = R.template triangularView<Upper>()*wa1 + qtf;
0261 temp = wa3.stableNorm();
0262 prered = 0.;
0263 if (temp < fnorm)
0264 prered = 1. - numext::abs2(temp / fnorm);
0265
0266
0267 ratio = 0.;
0268 if (prered > 0.)
0269 ratio = actred / prered;
0270
0271
0272 if (ratio < Scalar(.1)) {
0273 ncsuc = 0;
0274 ++ncfail;
0275 delta = Scalar(.5) * delta;
0276 } else {
0277 ncfail = 0;
0278 ++ncsuc;
0279 if (ratio >= Scalar(.5) || ncsuc > 1)
0280 delta = (std::max)(delta, pnorm / Scalar(.5));
0281 if (abs(ratio - 1.) <= Scalar(.1)) {
0282 delta = pnorm / Scalar(.5);
0283 }
0284 }
0285
0286
0287 if (ratio >= Scalar(1e-4)) {
0288
0289 x = wa2;
0290 wa2 = diag.cwiseProduct(x);
0291 fvec = wa4;
0292 xnorm = wa2.stableNorm();
0293 fnorm = fnorm1;
0294 ++iter;
0295 }
0296
0297
0298 ++nslow1;
0299 if (actred >= Scalar(.001))
0300 nslow1 = 0;
0301 if (jeval)
0302 ++nslow2;
0303 if (actred >= Scalar(.1))
0304 nslow2 = 0;
0305
0306
0307 if (delta <= parameters.xtol * xnorm || fnorm == 0.)
0308 return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
0309
0310
0311 if (nfev >= parameters.maxfev)
0312 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
0313 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
0314 return HybridNonLinearSolverSpace::TolTooSmall;
0315 if (nslow2 == 5)
0316 return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
0317 if (nslow1 == 10)
0318 return HybridNonLinearSolverSpace::NotMakingProgressIterations;
0319
0320
0321 if (ncfail == 2)
0322 break;
0323
0324
0325
0326 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
0327 wa2 = fjac.transpose() * wa4;
0328 if (ratio >= Scalar(1e-4))
0329 qtf = wa2;
0330 wa2 = (wa2-wa3)/pnorm;
0331
0332
0333 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
0334 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
0335 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
0336
0337 jeval = false;
0338 }
0339 return HybridNonLinearSolverSpace::Running;
0340 }
0341
0342 template<typename FunctorType, typename Scalar>
0343 HybridNonLinearSolverSpace::Status
0344 HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x)
0345 {
0346 HybridNonLinearSolverSpace::Status status = solveInit(x);
0347 if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
0348 return status;
0349 while (status==HybridNonLinearSolverSpace::Running)
0350 status = solveOneStep(x);
0351 return status;
0352 }
0353
0354
0355
0356 template<typename FunctorType, typename Scalar>
0357 HybridNonLinearSolverSpace::Status
0358 HybridNonLinearSolver<FunctorType,Scalar>::hybrd1(
0359 FVectorType &x,
0360 const Scalar tol
0361 )
0362 {
0363 n = x.size();
0364
0365
0366 if (n <= 0 || tol < 0.)
0367 return HybridNonLinearSolverSpace::ImproperInputParameters;
0368
0369 resetParameters();
0370 parameters.maxfev = 200*(n+1);
0371 parameters.xtol = tol;
0372
0373 diag.setConstant(n, 1.);
0374 useExternalScaling = true;
0375 return solveNumericalDiff(x);
0376 }
0377
0378 template<typename FunctorType, typename Scalar>
0379 HybridNonLinearSolverSpace::Status
0380 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x)
0381 {
0382 n = x.size();
0383
0384 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
0385 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
0386
0387 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
0388 qtf.resize(n);
0389 fjac.resize(n, n);
0390 fvec.resize(n);
0391 if (!useExternalScaling)
0392 diag.resize(n);
0393 eigen_assert( (!useExternalScaling || diag.size()==n) && "When useExternalScaling is set, the caller must provide a valid 'diag'");
0394
0395
0396 nfev = 0;
0397 njev = 0;
0398
0399
0400 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. )
0401 return HybridNonLinearSolverSpace::ImproperInputParameters;
0402 if (useExternalScaling)
0403 for (Index j = 0; j < n; ++j)
0404 if (diag[j] <= 0.)
0405 return HybridNonLinearSolverSpace::ImproperInputParameters;
0406
0407
0408
0409 nfev = 1;
0410 if ( functor(x, fvec) < 0)
0411 return HybridNonLinearSolverSpace::UserAsked;
0412 fnorm = fvec.stableNorm();
0413
0414
0415 iter = 1;
0416 ncsuc = 0;
0417 ncfail = 0;
0418 nslow1 = 0;
0419 nslow2 = 0;
0420
0421 return HybridNonLinearSolverSpace::Running;
0422 }
0423
0424 template<typename FunctorType, typename Scalar>
0425 HybridNonLinearSolverSpace::Status
0426 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x)
0427 {
0428 using std::sqrt;
0429 using std::abs;
0430
0431 assert(x.size()==n);
0432
0433 Index j;
0434 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
0435
0436 jeval = true;
0437 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
0438 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
0439
0440
0441 if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0)
0442 return HybridNonLinearSolverSpace::UserAsked;
0443 nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n);
0444
0445 wa2 = fjac.colwise().blueNorm();
0446
0447
0448
0449 if (iter == 1) {
0450 if (!useExternalScaling)
0451 for (j = 0; j < n; ++j)
0452 diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
0453
0454
0455
0456 xnorm = diag.cwiseProduct(x).stableNorm();
0457 delta = parameters.factor * xnorm;
0458 if (delta == 0.)
0459 delta = parameters.factor;
0460 }
0461
0462
0463 HouseholderQR<JacobianType> qrfac(fjac);
0464
0465
0466 R = qrfac.matrixQR();
0467
0468
0469 fjac = qrfac.householderQ();
0470
0471
0472 qtf = fjac.transpose() * fvec;
0473
0474
0475 if (!useExternalScaling)
0476 diag = diag.cwiseMax(wa2);
0477
0478 while (true) {
0479
0480 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
0481
0482
0483 wa1 = -wa1;
0484 wa2 = x + wa1;
0485 pnorm = diag.cwiseProduct(wa1).stableNorm();
0486
0487
0488 if (iter == 1)
0489 delta = (std::min)(delta,pnorm);
0490
0491
0492 if ( functor(wa2, wa4) < 0)
0493 return HybridNonLinearSolverSpace::UserAsked;
0494 ++nfev;
0495 fnorm1 = wa4.stableNorm();
0496
0497
0498 actred = -1.;
0499 if (fnorm1 < fnorm)
0500 actred = 1. - numext::abs2(fnorm1 / fnorm);
0501
0502
0503 wa3 = R.template triangularView<Upper>()*wa1 + qtf;
0504 temp = wa3.stableNorm();
0505 prered = 0.;
0506 if (temp < fnorm)
0507 prered = 1. - numext::abs2(temp / fnorm);
0508
0509
0510 ratio = 0.;
0511 if (prered > 0.)
0512 ratio = actred / prered;
0513
0514
0515 if (ratio < Scalar(.1)) {
0516 ncsuc = 0;
0517 ++ncfail;
0518 delta = Scalar(.5) * delta;
0519 } else {
0520 ncfail = 0;
0521 ++ncsuc;
0522 if (ratio >= Scalar(.5) || ncsuc > 1)
0523 delta = (std::max)(delta, pnorm / Scalar(.5));
0524 if (abs(ratio - 1.) <= Scalar(.1)) {
0525 delta = pnorm / Scalar(.5);
0526 }
0527 }
0528
0529
0530 if (ratio >= Scalar(1e-4)) {
0531
0532 x = wa2;
0533 wa2 = diag.cwiseProduct(x);
0534 fvec = wa4;
0535 xnorm = wa2.stableNorm();
0536 fnorm = fnorm1;
0537 ++iter;
0538 }
0539
0540
0541 ++nslow1;
0542 if (actred >= Scalar(.001))
0543 nslow1 = 0;
0544 if (jeval)
0545 ++nslow2;
0546 if (actred >= Scalar(.1))
0547 nslow2 = 0;
0548
0549
0550 if (delta <= parameters.xtol * xnorm || fnorm == 0.)
0551 return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
0552
0553
0554 if (nfev >= parameters.maxfev)
0555 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
0556 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
0557 return HybridNonLinearSolverSpace::TolTooSmall;
0558 if (nslow2 == 5)
0559 return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
0560 if (nslow1 == 10)
0561 return HybridNonLinearSolverSpace::NotMakingProgressIterations;
0562
0563
0564 if (ncfail == 2)
0565 break;
0566
0567
0568
0569 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
0570 wa2 = fjac.transpose() * wa4;
0571 if (ratio >= Scalar(1e-4))
0572 qtf = wa2;
0573 wa2 = (wa2-wa3)/pnorm;
0574
0575
0576 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
0577 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
0578 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
0579
0580 jeval = false;
0581 }
0582 return HybridNonLinearSolverSpace::Running;
0583 }
0584
0585 template<typename FunctorType, typename Scalar>
0586 HybridNonLinearSolverSpace::Status
0587 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x)
0588 {
0589 HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x);
0590 if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
0591 return status;
0592 while (status==HybridNonLinearSolverSpace::Running)
0593 status = solveNumericalDiffOneStep(x);
0594 return status;
0595 }
0596
0597 }
0598
0599 #endif
0600
0601