13 #ifndef EIGEN_HYBRIDNONLINEARSOLVER_H
14 #define EIGEN_HYBRIDNONLINEARSOLVER_H
18 namespace HybridNonLinearSolverSpace {
21 ImproperInputParameters = 0,
22 RelativeErrorTooSmall = 1,
23 TooManyFunctionEvaluation = 2,
25 NotMakingProgressJacobian = 4,
26 NotMakingProgressIterations = 5,
42 template<
typename FunctorType,
typename Scalar=
double>
46 typedef DenseIndex Index;
49 : functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; useExternalScaling=
false;}
53 : factor(Scalar(100.))
56 , nb_of_subdiagonals(-1)
57 , nb_of_superdiagonals(-1)
58 , epsfcn(Scalar(0.)) {}
62 Index nb_of_subdiagonals;
63 Index nb_of_superdiagonals;
71 HybridNonLinearSolverSpace::Status hybrj1(
76 HybridNonLinearSolverSpace::Status solveInit(
FVectorType &x);
77 HybridNonLinearSolverSpace::Status solveOneStep(
FVectorType &x);
78 HybridNonLinearSolverSpace::Status solve(
FVectorType &x);
80 HybridNonLinearSolverSpace::Status hybrd1(
85 HybridNonLinearSolverSpace::Status solveNumericalDiffInit(
FVectorType &x);
86 HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(
FVectorType &x);
87 HybridNonLinearSolverSpace::Status solveNumericalDiff(
FVectorType &x);
89 void resetParameters(
void) { parameters = Parameters(); }
90 Parameters parameters;
98 bool useExternalScaling;
100 FunctorType &functor;
109 Scalar pnorm, xnorm, fnorm1;
110 Index nslow1, nslow2;
112 Scalar actred, prered;
120 template<
typename FunctorType,
typename Scalar>
121 HybridNonLinearSolverSpace::Status
130 if (n <= 0 || tol < 0.)
131 return HybridNonLinearSolverSpace::ImproperInputParameters;
134 parameters.maxfev = 100*(n+1);
135 parameters.xtol = tol;
136 diag.setConstant(n, 1.);
137 useExternalScaling =
true;
141 template<
typename FunctorType,
typename Scalar>
142 HybridNonLinearSolverSpace::Status
143 HybridNonLinearSolver<FunctorType,Scalar>::solveInit(FVectorType &x)
147 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
151 if (!useExternalScaling)
153 assert( (!useExternalScaling || diag.size()==n) ||
"When useExternalScaling is set, the caller must provide a valid 'diag'");
160 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. )
161 return HybridNonLinearSolverSpace::ImproperInputParameters;
162 if (useExternalScaling)
163 for (Index j = 0; j < n; ++j)
165 return HybridNonLinearSolverSpace::ImproperInputParameters;
170 if ( functor(x, fvec) < 0)
171 return HybridNonLinearSolverSpace::UserAsked;
172 fnorm = fvec.stableNorm();
181 return HybridNonLinearSolverSpace::Running;
184 template<
typename FunctorType,
typename Scalar>
185 HybridNonLinearSolverSpace::Status
186 HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(FVectorType &x)
191 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
196 if ( functor.df(x, fjac) < 0)
197 return HybridNonLinearSolverSpace::UserAsked;
200 wa2 = fjac.colwise().blueNorm();
205 if (!useExternalScaling)
206 for (j = 0; j < n; ++j)
207 diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
211 xnorm = diag.cwiseProduct(x).stableNorm();
212 delta = parameters.factor * xnorm;
214 delta = parameters.factor;
218 HouseholderQR<JacobianType> qrfac(fjac);
221 R = qrfac.matrixQR();
224 fjac = qrfac.householderQ();
227 qtf = fjac.transpose() * fvec;
230 if (!useExternalScaling)
231 diag = diag.cwiseMax(wa2);
235 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
240 pnorm = diag.cwiseProduct(wa1).stableNorm();
244 delta = (std::min)(delta,pnorm);
247 if ( functor(wa2, wa4) < 0)
248 return HybridNonLinearSolverSpace::UserAsked;
250 fnorm1 = wa4.stableNorm();
255 actred = 1. - internal::abs2(fnorm1 / fnorm);
258 wa3 = R.template triangularView<Upper>()*wa1 + qtf;
259 temp = wa3.stableNorm();
262 prered = 1. - internal::abs2(temp / fnorm);
267 ratio = actred / prered;
270 if (ratio < Scalar(.1)) {
273 delta = Scalar(.5) * delta;
277 if (ratio >= Scalar(.5) || ncsuc > 1)
278 delta = (std::max)(delta, pnorm / Scalar(.5));
279 if (internal::abs(ratio - 1.) <= Scalar(.1)) {
280 delta = pnorm / Scalar(.5);
285 if (ratio >= Scalar(1e-4)) {
288 wa2 = diag.cwiseProduct(x);
290 xnorm = wa2.stableNorm();
297 if (actred >= Scalar(.001))
301 if (actred >= Scalar(.1))
305 if (delta <= parameters.xtol * xnorm || fnorm == 0.)
306 return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
309 if (nfev >= parameters.maxfev)
310 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
311 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
312 return HybridNonLinearSolverSpace::TolTooSmall;
314 return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
316 return HybridNonLinearSolverSpace::NotMakingProgressIterations;
324 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
325 wa2 = fjac.transpose() * wa4;
326 if (ratio >= Scalar(1e-4))
328 wa2 = (wa2-wa3)/pnorm;
331 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
332 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
333 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
337 return HybridNonLinearSolverSpace::Running;
340 template<
typename FunctorType,
typename Scalar>
341 HybridNonLinearSolverSpace::Status
342 HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x)
344 HybridNonLinearSolverSpace::Status status = solveInit(x);
345 if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
347 while (status==HybridNonLinearSolverSpace::Running)
348 status = solveOneStep(x);
354 template<
typename FunctorType,
typename Scalar>
355 HybridNonLinearSolverSpace::Status
356 HybridNonLinearSolver<FunctorType,Scalar>::hybrd1(
364 if (n <= 0 || tol < 0.)
365 return HybridNonLinearSolverSpace::ImproperInputParameters;
368 parameters.maxfev = 200*(n+1);
369 parameters.xtol = tol;
371 diag.setConstant(n, 1.);
372 useExternalScaling =
true;
373 return solveNumericalDiff(x);
376 template<
typename FunctorType,
typename Scalar>
377 HybridNonLinearSolverSpace::Status
378 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x)
382 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
383 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
385 wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
389 if (!useExternalScaling)
391 assert( (!useExternalScaling || diag.size()==n) ||
"When useExternalScaling is set, the caller must provide a valid 'diag'");
398 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. )
399 return HybridNonLinearSolverSpace::ImproperInputParameters;
400 if (useExternalScaling)
401 for (Index j = 0; j < n; ++j)
403 return HybridNonLinearSolverSpace::ImproperInputParameters;
408 if ( functor(x, fvec) < 0)
409 return HybridNonLinearSolverSpace::UserAsked;
410 fnorm = fvec.stableNorm();
419 return HybridNonLinearSolverSpace::Running;
422 template<
typename FunctorType,
typename Scalar>
423 HybridNonLinearSolverSpace::Status
424 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x)
429 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
432 if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
433 if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
436 if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0)
437 return HybridNonLinearSolverSpace::UserAsked;
438 nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n);
440 wa2 = fjac.colwise().blueNorm();
445 if (!useExternalScaling)
446 for (j = 0; j < n; ++j)
447 diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
451 xnorm = diag.cwiseProduct(x).stableNorm();
452 delta = parameters.factor * xnorm;
454 delta = parameters.factor;
458 HouseholderQR<JacobianType> qrfac(fjac);
461 R = qrfac.matrixQR();
464 fjac = qrfac.householderQ();
467 qtf = fjac.transpose() * fvec;
470 if (!useExternalScaling)
471 diag = diag.cwiseMax(wa2);
475 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
480 pnorm = diag.cwiseProduct(wa1).stableNorm();
484 delta = (std::min)(delta,pnorm);
487 if ( functor(wa2, wa4) < 0)
488 return HybridNonLinearSolverSpace::UserAsked;
490 fnorm1 = wa4.stableNorm();
495 actred = 1. - internal::abs2(fnorm1 / fnorm);
498 wa3 = R.template triangularView<Upper>()*wa1 + qtf;
499 temp = wa3.stableNorm();
502 prered = 1. - internal::abs2(temp / fnorm);
507 ratio = actred / prered;
510 if (ratio < Scalar(.1)) {
513 delta = Scalar(.5) * delta;
517 if (ratio >= Scalar(.5) || ncsuc > 1)
518 delta = (std::max)(delta, pnorm / Scalar(.5));
519 if (internal::abs(ratio - 1.) <= Scalar(.1)) {
520 delta = pnorm / Scalar(.5);
525 if (ratio >= Scalar(1e-4)) {
528 wa2 = diag.cwiseProduct(x);
530 xnorm = wa2.stableNorm();
537 if (actred >= Scalar(.001))
541 if (actred >= Scalar(.1))
545 if (delta <= parameters.xtol * xnorm || fnorm == 0.)
546 return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
549 if (nfev >= parameters.maxfev)
550 return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
551 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
552 return HybridNonLinearSolverSpace::TolTooSmall;
554 return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
556 return HybridNonLinearSolverSpace::NotMakingProgressIterations;
564 wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
565 wa2 = fjac.transpose() * wa4;
566 if (ratio >= Scalar(1e-4))
568 wa2 = (wa2-wa3)/pnorm;
571 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
572 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
573 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
577 return HybridNonLinearSolverSpace::Running;
580 template<
typename FunctorType,
typename Scalar>
581 HybridNonLinearSolverSpace::Status
582 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x)
584 HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x);
585 if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
587 while (status==HybridNonLinearSolverSpace::Running)
588 status = solveNumericalDiffOneStep(x);
594 #endif // EIGEN_HYBRIDNONLINEARSOLVER_H