28 #ifndef EIGEN_HYBRIDNONLINEARSOLVER_H
29 #define EIGEN_HYBRIDNONLINEARSOLVER_H
33 namespace HybridNonLinearSolverSpace {
36 ImproperInputParameters = 0,
37 RelativeErrorTooSmall = 1,
38 TooManyFunctionEvaluation = 2,
40 NotMakingProgressJacobian = 4,
41 NotMakingProgressIterations = 5,
57 template<
typename FunctorType,
typename Scalar=
double>
61 typedef DenseIndex Index;
64 : functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; useExternalScaling=
false;}
68 : factor(Scalar(100.))
70 , xtol(internal::sqrt(NumTraits<Scalar>::epsilon()))
71 , nb_of_subdiagonals(-1)
72 , nb_of_superdiagonals(-1)
73 , epsfcn(Scalar(0.)) {}
77 Index nb_of_subdiagonals;
78 Index nb_of_superdiagonals;
81 typedef Matrix< Scalar, Dynamic, 1 > FVectorType;
82 typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType;
84 typedef Matrix< Scalar, Dynamic, Dynamic > UpperTriangularType;
86 HybridNonLinearSolverSpace::Status hybrj1(
88 const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon())
91 HybridNonLinearSolverSpace::Status solveInit(FVectorType &x);
92 HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x);
93 HybridNonLinearSolverSpace::Status solve(FVectorType &x);
95 HybridNonLinearSolverSpace::Status hybrd1(
97 const Scalar tol = internal::sqrt(NumTraits<Scalar>::epsilon())
100 HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x);
101 HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x);
102 HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x);
104 void resetParameters(
void) { parameters = Parameters(); }
105 Parameters parameters;
106 FVectorType fvec, qtf, diag;
108 UpperTriangularType R;
113 bool useExternalScaling;
115 FunctorType &functor;
124 Scalar pnorm, xnorm, fnorm1;
125 Index nslow1, nslow2;
127 Scalar actred, prered;
128 FVectorType wa1, wa2, wa3, wa4;
135 template<
typename FunctorType,
typename Scalar>
136 HybridNonLinearSolverSpace::Status
145 if (n <= 0 || tol < 0.)
146 return HybridNonLinearSolverSpace::ImproperInputParameters;
149 parameters.maxfev = 100*(n+1);
150 parameters.xtol = tol;
151 diag.setConstant(n, 1.);
152 useExternalScaling =
true;
156 template<
typename FunctorType,
typename Scalar>
157 HybridNonLinearSolverSpace::Status
158 HybridNonLinearSolver<FunctorType,Scalar>::solveInit(FVectorType &x)
162 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
166 if (!useExternalScaling)
168 assert( (!useExternalScaling || diag.size()==n) ||
"When useExternalScaling is set, the caller must provide a valid 'diag'");
175 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. )
176 return HybridNonLinearSolverSpace::ImproperInputParameters;
177 if (useExternalScaling)
178 for (Index j = 0; j < n; ++j)
180 return HybridNonLinearSolverSpace::ImproperInputParameters;
185 if ( functor(x, fvec) < 0)
186 return HybridNonLinearSolverSpace::UserAsked;
187 fnorm = fvec.stableNorm();
196 return HybridNonLinearSolverSpace::Running;
199 template<
typename FunctorType,
typename Scalar>
200 HybridNonLinearSolverSpace::Status
201 HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(FVectorType &x)
206 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
211 if ( functor.df(x, fjac) < 0)
212 return HybridNonLinearSolverSpace::UserAsked;
215 wa2 = fjac.colwise().blueNorm();
220 if (!useExternalScaling)
221 for (j = 0; j < n; ++j)
222 diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
226 xnorm = diag.cwiseProduct(x).stableNorm();
227 delta = parameters.factor * xnorm;
229 delta = parameters.factor;
233 HouseholderQR<JacobianType> qrfac(fjac);
236 R = qrfac.matrixQR();
239 fjac = qrfac.householderQ();
242 qtf = fjac.transpose() * fvec;
245 if (!useExternalScaling)
246 diag = diag.cwiseMax(wa2);
250 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
255 pnorm = diag.cwiseProduct(wa1).stableNorm();
259 delta = (std::min)(delta,pnorm);
262 if ( functor(wa2, wa4) < 0)
263 return HybridNonLinearSolverSpace::UserAsked;
265 fnorm1 = wa4.stableNorm();
270 actred = 1. - internal::abs2(fnorm1 / fnorm);
273 wa3 = R.template triangularView<Upper>()*wa1 + qtf;
274 temp = wa3.stableNorm();
277 prered = 1. - internal::abs2(temp / fnorm);
282 ratio = actred / prered;
285 if (ratio < Scalar(.1)) {
288 delta = Scalar(.5) * delta;
292 if (ratio >= Scalar(.5) || ncsuc > 1)
293 delta = (std::max)(delta, pnorm / Scalar(.5));
294 if (internal::abs(ratio - 1.) <= Scalar(.1)) {
295 delta = pnorm / Scalar(.5);
300 if (ratio >= Scalar(1e-4)) {
303 wa2 = diag.cwiseProduct(x);
305 xnorm = wa2.stableNorm();
312 if (actred >= Scalar(.001))
316 if (actred >= Scalar(.1))
320 if (delta <= parameters.xtol * xnorm || fnorm == 0.)
321 return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
324 if (nfev >= parameters.maxfev)
325 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
326 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
327 return HybridNonLinearSolverSpace::TolTooSmall;
329 return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
331 return HybridNonLinearSolverSpace::NotMakingProgressIterations;
339 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
340 wa2 = fjac.transpose() * wa4;
341 if (ratio >= Scalar(1e-4))
343 wa2 = (wa2-wa3)/pnorm;
346 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
347 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
348 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
352 return HybridNonLinearSolverSpace::Running;
355 template<
typename FunctorType,
typename Scalar>
356 HybridNonLinearSolverSpace::Status
357 HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x)
359 HybridNonLinearSolverSpace::Status status = solveInit(x);
360 if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
362 while (status==HybridNonLinearSolverSpace::Running)
363 status = solveOneStep(x);
369 template<
typename FunctorType,
typename Scalar>
370 HybridNonLinearSolverSpace::Status
371 HybridNonLinearSolver<FunctorType,Scalar>::hybrd1(
379 if (n <= 0 || tol < 0.)
380 return HybridNonLinearSolverSpace::ImproperInputParameters;
383 parameters.maxfev = 200*(n+1);
384 parameters.xtol = tol;
386 diag.setConstant(n, 1.);
387 useExternalScaling =
true;
388 return solveNumericalDiff(x);
391 template<
typename FunctorType,
typename Scalar>
392 HybridNonLinearSolverSpace::Status
393 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x)
397 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
398 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
400 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
404 if (!useExternalScaling)
406 assert( (!useExternalScaling || diag.size()==n) ||
"When useExternalScaling is set, the caller must provide a valid 'diag'");
413 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. )
414 return HybridNonLinearSolverSpace::ImproperInputParameters;
415 if (useExternalScaling)
416 for (Index j = 0; j < n; ++j)
418 return HybridNonLinearSolverSpace::ImproperInputParameters;
423 if ( functor(x, fvec) < 0)
424 return HybridNonLinearSolverSpace::UserAsked;
425 fnorm = fvec.stableNorm();
434 return HybridNonLinearSolverSpace::Running;
437 template<
typename FunctorType,
typename Scalar>
438 HybridNonLinearSolverSpace::Status
439 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x)
444 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
447 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
448 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
451 if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0)
452 return HybridNonLinearSolverSpace::UserAsked;
453 nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n);
455 wa2 = fjac.colwise().blueNorm();
460 if (!useExternalScaling)
461 for (j = 0; j < n; ++j)
462 diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
466 xnorm = diag.cwiseProduct(x).stableNorm();
467 delta = parameters.factor * xnorm;
469 delta = parameters.factor;
473 HouseholderQR<JacobianType> qrfac(fjac);
476 R = qrfac.matrixQR();
479 fjac = qrfac.householderQ();
482 qtf = fjac.transpose() * fvec;
485 if (!useExternalScaling)
486 diag = diag.cwiseMax(wa2);
490 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
495 pnorm = diag.cwiseProduct(wa1).stableNorm();
499 delta = (std::min)(delta,pnorm);
502 if ( functor(wa2, wa4) < 0)
503 return HybridNonLinearSolverSpace::UserAsked;
505 fnorm1 = wa4.stableNorm();
510 actred = 1. - internal::abs2(fnorm1 / fnorm);
513 wa3 = R.template triangularView<Upper>()*wa1 + qtf;
514 temp = wa3.stableNorm();
517 prered = 1. - internal::abs2(temp / fnorm);
522 ratio = actred / prered;
525 if (ratio < Scalar(.1)) {
528 delta = Scalar(.5) * delta;
532 if (ratio >= Scalar(.5) || ncsuc > 1)
533 delta = (std::max)(delta, pnorm / Scalar(.5));
534 if (internal::abs(ratio - 1.) <= Scalar(.1)) {
535 delta = pnorm / Scalar(.5);
540 if (ratio >= Scalar(1e-4)) {
543 wa2 = diag.cwiseProduct(x);
545 xnorm = wa2.stableNorm();
552 if (actred >= Scalar(.001))
556 if (actred >= Scalar(.1))
560 if (delta <= parameters.xtol * xnorm || fnorm == 0.)
561 return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
564 if (nfev >= parameters.maxfev)
565 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
566 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
567 return HybridNonLinearSolverSpace::TolTooSmall;
569 return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
571 return HybridNonLinearSolverSpace::NotMakingProgressIterations;
579 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
580 wa2 = fjac.transpose() * wa4;
581 if (ratio >= Scalar(1e-4))
583 wa2 = (wa2-wa3)/pnorm;
586 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
587 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
588 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
592 return HybridNonLinearSolverSpace::Running;
595 template<
typename FunctorType,
typename Scalar>
596 HybridNonLinearSolverSpace::Status
597 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x)
599 HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x);
600 if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
602 while (status==HybridNonLinearSolverSpace::Running)
603 status = solveNumericalDiffOneStep(x);
609 #endif // EIGEN_HYBRIDNONLINEARSOLVER_H