Eigen-Contrib  5.0.1
 
Loading...
Searching...
No Matches
HybridNonLinearSolver.h
1// -*- coding: utf-8
2// vim: set fileencoding=utf-8
3// SPDX-License-Identifier: MPL-2.0
4
5// This file is part of Eigen, a lightweight C++ template library
6// for linear algebra.
7//
8// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
9//
10// This Source Code Form is subject to the terms of the Mozilla
11// Public License v. 2.0. If a copy of the MPL was not distributed
12// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
13
14#ifndef EIGEN_HYBRIDNONLINEARSOLVER_H
15#define EIGEN_HYBRIDNONLINEARSOLVER_H
16
17// IWYU pragma: private
18#include "./InternalHeaderCheck.h"
19
20namespace Eigen {
21
22namespace HybridNonLinearSolverSpace {
23enum Status {
24 Running = -1,
25 ImproperInputParameters = 0,
26 RelativeErrorTooSmall = 1,
27 TooManyFunctionEvaluation = 2,
28 TolTooSmall = 3,
29 NotMakingProgressJacobian = 4,
30 NotMakingProgressIterations = 5,
31 UserAsked = 6
32};
33}
34
46template <typename FunctorType, typename Scalar = double>
47class HybridNonLinearSolver {
48 public:
49 typedef DenseIndex Index;
50
51 HybridNonLinearSolver(FunctorType &_functor) : functor(_functor) {
52 nfev = njev = iter = 0;
53 fnorm = 0.;
54 useExternalScaling = false;
55 }
56
57 struct Parameters {
58 Parameters()
59 : factor(Scalar(100.)),
60 maxfev(1000),
61 xtol(numext::sqrt(NumTraits<Scalar>::epsilon())),
62 nb_of_subdiagonals(-1),
63 nb_of_superdiagonals(-1),
64 epsfcn(Scalar(0.)) {}
65 Scalar factor;
66 Index maxfev; // maximum number of function evaluations
67 Scalar xtol;
68 Index nb_of_subdiagonals;
69 Index nb_of_superdiagonals;
70 Scalar epsfcn;
71 };
72 typedef Matrix<Scalar, Dynamic, 1> FVectorType;
73 typedef Matrix<Scalar, Dynamic, Dynamic> JacobianType;
74 /* TODO: if eigen provides a triangular storage, use it here */
75 typedef Matrix<Scalar, Dynamic, Dynamic> UpperTriangularType;
76
77 HybridNonLinearSolverSpace::Status hybrj1(FVectorType &x,
78 const Scalar tol = numext::sqrt(NumTraits<Scalar>::epsilon()));
79
80 HybridNonLinearSolverSpace::Status solveInit(FVectorType &x);
81 HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x);
82 HybridNonLinearSolverSpace::Status solve(FVectorType &x);
83
84 HybridNonLinearSolverSpace::Status hybrd1(FVectorType &x,
85 const Scalar tol = numext::sqrt(NumTraits<Scalar>::epsilon()));
86
87 HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x);
88 HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x);
89 HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x);
90
91 void resetParameters(void) { parameters = Parameters(); }
92 Parameters parameters;
93 FVectorType fvec, qtf, diag;
94 JacobianType fjac;
95 UpperTriangularType R;
96 Index nfev;
97 Index njev;
98 Index iter;
99 Scalar fnorm;
100 bool useExternalScaling;
101
102 private:
103 FunctorType &functor;
104 Index n;
105 bool sing;
106 Scalar temp;
107 Scalar delta;
108 bool jeval;
109 Index ncsuc;
110 Scalar ratio;
111 Scalar pnorm, xnorm, fnorm1;
112 Index nslow1, nslow2;
113 Index ncfail;
114 Scalar actred, prered;
115 FVectorType wa1, wa2, wa3, wa4;
116
117 HybridNonLinearSolver &operator=(const HybridNonLinearSolver &) = delete;
118};
119
120template <typename FunctorType, typename Scalar>
121HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::hybrj1(FVectorType &x,
122 const Scalar tol) {
123 n = x.size();
124
125 /* check the input parameters for errors. */
126 if (n <= 0 || tol < 0.) return HybridNonLinearSolverSpace::ImproperInputParameters;
127
128 resetParameters();
129 parameters.maxfev = 100 * (n + 1);
130 parameters.xtol = tol;
131 diag.setConstant(n, 1.);
132 useExternalScaling = true;
133 return solve(x);
134}
135
136template <typename FunctorType, typename Scalar>
137HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solveInit(FVectorType &x) {
138 n = x.size();
139
140 wa1.resize(n);
141 wa2.resize(n);
142 wa3.resize(n);
143 wa4.resize(n);
144 fvec.resize(n);
145 qtf.resize(n);
146 fjac.resize(n, n);
147 if (!useExternalScaling) diag.resize(n);
148 eigen_assert((!useExternalScaling || diag.size() == n) &&
149 "When useExternalScaling is set, the caller must provide a valid 'diag'");
150
151 /* Function Body */
152 nfev = 0;
153 njev = 0;
154
155 /* check the input parameters for errors. */
156 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0.)
157 return HybridNonLinearSolverSpace::ImproperInputParameters;
158 if (useExternalScaling)
159 for (Index j = 0; j < n; ++j)
160 if (diag[j] <= 0.) return HybridNonLinearSolverSpace::ImproperInputParameters;
161
162 /* evaluate the function at the starting point */
163 /* and calculate its norm. */
164 nfev = 1;
165 if (functor(x, fvec) < 0) return HybridNonLinearSolverSpace::UserAsked;
166 fnorm = fvec.stableNorm();
167
168 /* initialize iteration counter and monitors. */
169 iter = 1;
170 ncsuc = 0;
171 ncfail = 0;
172 nslow1 = 0;
173 nslow2 = 0;
174
175 return HybridNonLinearSolverSpace::Running;
176}
177
178template <typename FunctorType, typename Scalar>
179HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solveOneStep(FVectorType &x) {
180 using std::abs;
181
182 eigen_assert(x.size() == n); // check the caller is not cheating us
183
184 Index j;
185 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
186
187 jeval = true;
188
189 /* calculate the jacobian matrix. */
190 if (functor.df(x, fjac) < 0) return HybridNonLinearSolverSpace::UserAsked;
191 ++njev;
192
193 wa2 = fjac.colwise().blueNorm();
194
195 /* on the first iteration and if external scaling is not used, scale according */
196 /* to the norms of the columns of the initial jacobian. */
197 if (iter == 1) {
198 if (!useExternalScaling)
199 for (j = 0; j < n; ++j) diag[j] = (wa2[j] == 0.) ? 1. : wa2[j];
200
201 /* on the first iteration, calculate the norm of the scaled x */
202 /* and initialize the step bound delta. */
203 xnorm = diag.cwiseProduct(x).stableNorm();
204 delta = parameters.factor * xnorm;
205 if (delta == 0.) delta = parameters.factor;
206 }
207
208 /* compute the qr factorization of the jacobian. */
209 HouseholderQR<JacobianType> qrfac(fjac); // no pivoting:
210
211 /* copy the triangular factor of the qr factorization into r. */
212 R = qrfac.matrixQR();
213
214 /* accumulate the orthogonal factor in fjac. */
215 fjac = qrfac.householderQ();
216
217 /* form (q transpose)*fvec and store in qtf. */
218 qtf.noalias() = fjac.transpose() * fvec;
219
220 /* rescale if necessary. */
221 if (!useExternalScaling) diag = diag.cwiseMax(wa2);
222
223 while (true) {
224 /* determine the direction p. */
225 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
226
227 /* store the direction p and x + p. calculate the norm of p. */
228 wa1 = -wa1;
229 wa2 = x + wa1;
230 pnorm = diag.cwiseProduct(wa1).stableNorm();
231
232 /* on the first iteration, adjust the initial step bound. */
233 if (iter == 1) delta = (std::min)(delta, pnorm);
234
235 /* evaluate the function at x + p and calculate its norm. */
236 if (functor(wa2, wa4) < 0) return HybridNonLinearSolverSpace::UserAsked;
237 ++nfev;
238 fnorm1 = wa4.stableNorm();
239
240 /* compute the scaled actual reduction. */
241 actred = -1.;
242 if (fnorm1 < fnorm) /* Computing 2nd power */
243 actred = 1. - numext::abs2(fnorm1 / fnorm);
244
245 /* compute the scaled predicted reduction. */
246 wa3.noalias() = R.template triangularView<Upper>() * wa1 + qtf;
247 temp = wa3.stableNorm();
248 prered = 0.;
249 if (temp < fnorm) /* Computing 2nd power */
250 prered = 1. - numext::abs2(temp / fnorm);
251
252 /* compute the ratio of the actual to the predicted reduction. */
253 ratio = 0.;
254 if (prered > 0.) ratio = actred / prered;
255
256 /* update the step bound. */
257 if (ratio < Scalar(.1)) {
258 ncsuc = 0;
259 ++ncfail;
260 delta = Scalar(.5) * delta;
261 } else {
262 ncfail = 0;
263 ++ncsuc;
264 if (ratio >= Scalar(.5) || ncsuc > 1) delta = (std::max)(delta, pnorm / Scalar(.5));
265 if (abs(ratio - 1.) <= Scalar(.1)) {
266 delta = pnorm / Scalar(.5);
267 }
268 }
269
270 /* test for successful iteration. */
271 if (ratio >= Scalar(1e-4)) {
272 /* successful iteration. update x, fvec, and their norms. */
273 x = wa2;
274 wa2 = diag.cwiseProduct(x);
275 fvec = wa4;
276 xnorm = wa2.stableNorm();
277 fnorm = fnorm1;
278 ++iter;
279 }
280
281 /* determine the progress of the iteration. */
282 ++nslow1;
283 if (actred >= Scalar(.001)) nslow1 = 0;
284 if (jeval) ++nslow2;
285 if (actred >= Scalar(.1)) nslow2 = 0;
286
287 /* test for convergence. */
288 if (delta <= parameters.xtol * xnorm || fnorm == 0.) return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
289
290 /* tests for termination and stringent tolerances. */
291 if (nfev >= parameters.maxfev) return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
292 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
293 return HybridNonLinearSolverSpace::TolTooSmall;
294 if (nslow2 == 5) return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
295 if (nslow1 == 10) return HybridNonLinearSolverSpace::NotMakingProgressIterations;
296
297 /* criterion for recalculating jacobian. */
298 if (ncfail == 2) break; // leave inner loop and go for the next outer loop iteration
299
300 /* calculate the rank one modification to the jacobian */
301 /* and update qtf if necessary. */
302 wa1 = diag.cwiseProduct(diag.cwiseProduct(wa1) / pnorm);
303 wa2.noalias() = fjac.transpose() * wa4;
304 if (ratio >= Scalar(1e-4)) qtf = wa2;
305 wa2 = (wa2 - wa3) / pnorm;
306
307 /* compute the qr factorization of the updated jacobian. */
308 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
309 internal::r1mpyq<Scalar>(fjac, v_givens, w_givens);
310 Transpose<FVectorType> qtf_row = qtf.transpose();
311 internal::r1mpyq<Scalar>(qtf_row, v_givens, w_givens);
312
313 jeval = false;
314 }
315 return HybridNonLinearSolverSpace::Running;
316}
317
318template <typename FunctorType, typename Scalar>
319HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solve(FVectorType &x) {
320 HybridNonLinearSolverSpace::Status status = solveInit(x);
321 if (status == HybridNonLinearSolverSpace::ImproperInputParameters) return status;
322 while (status == HybridNonLinearSolverSpace::Running) status = solveOneStep(x);
323 return status;
324}
325
326template <typename FunctorType, typename Scalar>
327HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::hybrd1(FVectorType &x,
328 const Scalar tol) {
329 n = x.size();
330
331 /* check the input parameters for errors. */
332 if (n <= 0 || tol < 0.) return HybridNonLinearSolverSpace::ImproperInputParameters;
333
334 resetParameters();
335 parameters.maxfev = 200 * (n + 1);
336 parameters.xtol = tol;
337
338 diag.setConstant(n, 1.);
339 useExternalScaling = true;
340 return solveNumericalDiff(x);
341}
342
343template <typename FunctorType, typename Scalar>
344HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solveNumericalDiffInit(FVectorType &x) {
345 n = x.size();
346
347 if (parameters.nb_of_subdiagonals < 0) parameters.nb_of_subdiagonals = n - 1;
348 if (parameters.nb_of_superdiagonals < 0) parameters.nb_of_superdiagonals = n - 1;
349
350 wa1.resize(n);
351 wa2.resize(n);
352 wa3.resize(n);
353 wa4.resize(n);
354 qtf.resize(n);
355 fjac.resize(n, n);
356 fvec.resize(n);
357 if (!useExternalScaling) diag.resize(n);
358 eigen_assert((!useExternalScaling || diag.size() == n) &&
359 "When useExternalScaling is set, the caller must provide a valid 'diag'");
360
361 /* Function Body */
362 nfev = 0;
363 njev = 0;
364
365 /* check the input parameters for errors. */
366 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals < 0 ||
367 parameters.nb_of_superdiagonals < 0 || parameters.factor <= 0.)
368 return HybridNonLinearSolverSpace::ImproperInputParameters;
369 if (useExternalScaling)
370 for (Index j = 0; j < n; ++j)
371 if (diag[j] <= 0.) return HybridNonLinearSolverSpace::ImproperInputParameters;
372
373 /* evaluate the function at the starting point */
374 /* and calculate its norm. */
375 nfev = 1;
376 if (functor(x, fvec) < 0) return HybridNonLinearSolverSpace::UserAsked;
377 fnorm = fvec.stableNorm();
378
379 /* initialize iteration counter and monitors. */
380 iter = 1;
381 ncsuc = 0;
382 ncfail = 0;
383 nslow1 = 0;
384 nslow2 = 0;
385
386 return HybridNonLinearSolverSpace::Running;
387}
388
389template <typename FunctorType, typename Scalar>
390HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solveNumericalDiffOneStep(
391 FVectorType &x) {
392 using std::abs;
393 using std::sqrt;
394
395 eigen_assert(x.size() == n); // check the caller is not cheating us
396
397 Index j;
398 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
399
400 jeval = true;
401 if (parameters.nb_of_subdiagonals < 0) parameters.nb_of_subdiagonals = n - 1;
402 if (parameters.nb_of_superdiagonals < 0) parameters.nb_of_superdiagonals = n - 1;
403
404 /* calculate the jacobian matrix. */
405 if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals,
406 parameters.epsfcn) < 0)
407 return HybridNonLinearSolverSpace::UserAsked;
408 nfev += (std::min)(parameters.nb_of_subdiagonals + parameters.nb_of_superdiagonals + 1, n);
409
410 wa2 = fjac.colwise().blueNorm();
411
412 /* on the first iteration and if external scaling is not used, scale according */
413 /* to the norms of the columns of the initial jacobian. */
414 if (iter == 1) {
415 if (!useExternalScaling)
416 for (j = 0; j < n; ++j) diag[j] = (wa2[j] == 0.) ? 1. : wa2[j];
417
418 /* on the first iteration, calculate the norm of the scaled x */
419 /* and initialize the step bound delta. */
420 xnorm = diag.cwiseProduct(x).stableNorm();
421 delta = parameters.factor * xnorm;
422 if (delta == 0.) delta = parameters.factor;
423 }
424
425 /* compute the qr factorization of the jacobian. */
426 HouseholderQR<JacobianType> qrfac(fjac); // no pivoting:
427
428 /* copy the triangular factor of the qr factorization into r. */
429 R = qrfac.matrixQR();
430
431 /* accumulate the orthogonal factor in fjac. */
432 fjac = qrfac.householderQ();
433
434 /* form (q transpose)*fvec and store in qtf. */
435 qtf.noalias() = fjac.transpose() * fvec;
436
437 /* rescale if necessary. */
438 if (!useExternalScaling) diag = diag.cwiseMax(wa2);
439
440 while (true) {
441 /* determine the direction p. */
442 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
443
444 /* store the direction p and x + p. calculate the norm of p. */
445 wa1 = -wa1;
446 wa2 = x + wa1;
447 pnorm = diag.cwiseProduct(wa1).stableNorm();
448
449 /* on the first iteration, adjust the initial step bound. */
450 if (iter == 1) delta = (std::min)(delta, pnorm);
451
452 /* evaluate the function at x + p and calculate its norm. */
453 if (functor(wa2, wa4) < 0) return HybridNonLinearSolverSpace::UserAsked;
454 ++nfev;
455 fnorm1 = wa4.stableNorm();
456
457 /* compute the scaled actual reduction. */
458 actred = -1.;
459 if (fnorm1 < fnorm) /* Computing 2nd power */
460 actred = 1. - numext::abs2(fnorm1 / fnorm);
461
462 /* compute the scaled predicted reduction. */
463 wa3.noalias() = R.template triangularView<Upper>() * wa1 + qtf;
464 temp = wa3.stableNorm();
465 prered = 0.;
466 if (temp < fnorm) /* Computing 2nd power */
467 prered = 1. - numext::abs2(temp / fnorm);
468
469 /* compute the ratio of the actual to the predicted reduction. */
470 ratio = 0.;
471 if (prered > 0.) ratio = actred / prered;
472
473 /* update the step bound. */
474 if (ratio < Scalar(.1)) {
475 ncsuc = 0;
476 ++ncfail;
477 delta = Scalar(.5) * delta;
478 } else {
479 ncfail = 0;
480 ++ncsuc;
481 if (ratio >= Scalar(.5) || ncsuc > 1) delta = (std::max)(delta, pnorm / Scalar(.5));
482 if (abs(ratio - 1.) <= Scalar(.1)) {
483 delta = pnorm / Scalar(.5);
484 }
485 }
486
487 /* test for successful iteration. */
488 if (ratio >= Scalar(1e-4)) {
489 /* successful iteration. update x, fvec, and their norms. */
490 x = wa2;
491 wa2 = diag.cwiseProduct(x);
492 fvec = wa4;
493 xnorm = wa2.stableNorm();
494 fnorm = fnorm1;
495 ++iter;
496 }
497
498 /* determine the progress of the iteration. */
499 ++nslow1;
500 if (actred >= Scalar(.001)) nslow1 = 0;
501 if (jeval) ++nslow2;
502 if (actred >= Scalar(.1)) nslow2 = 0;
503
504 /* test for convergence. */
505 if (delta <= parameters.xtol * xnorm || fnorm == 0.) return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
506
507 /* tests for termination and stringent tolerances. */
508 if (nfev >= parameters.maxfev) return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
509 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
510 return HybridNonLinearSolverSpace::TolTooSmall;
511 if (nslow2 == 5) return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
512 if (nslow1 == 10) return HybridNonLinearSolverSpace::NotMakingProgressIterations;
513
514 /* criterion for recalculating jacobian. */
515 if (ncfail == 2) break; // leave inner loop and go for the next outer loop iteration
516
517 /* calculate the rank one modification to the jacobian */
518 /* and update qtf if necessary. */
519 wa1 = diag.cwiseProduct(diag.cwiseProduct(wa1) / pnorm);
520 wa2.noalias() = fjac.transpose() * wa4;
521 if (ratio >= Scalar(1e-4)) qtf = wa2;
522 wa2 = (wa2 - wa3) / pnorm;
523
524 /* compute the qr factorization of the updated jacobian. */
525 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
526 internal::r1mpyq<Scalar>(fjac, v_givens, w_givens);
527 Transpose<FVectorType> qtf_row = qtf.transpose();
528 internal::r1mpyq<Scalar>(qtf_row, v_givens, w_givens);
529
530 jeval = false;
531 }
532 return HybridNonLinearSolverSpace::Running;
533}
534
535template <typename FunctorType, typename Scalar>
536HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solveNumericalDiff(FVectorType &x) {
537 HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x);
538 if (status == HybridNonLinearSolverSpace::ImproperInputParameters) return status;
539 while (status == HybridNonLinearSolverSpace::Running) status = solveNumericalDiffOneStep(x);
540 return status;
541}
542
543} // end namespace Eigen
544
545#endif // EIGEN_HYBRIDNONLINEARSOLVER_H
546
547// vim: ai ts=4 sts=4 et sw=4
Eigen
Namespace containing all symbols from the Eigen library.