Eigen-unsupported  5.0.1-dev+284dcc12
 
Loading...
Searching...
No Matches
HybridNonLinearSolver.h
1// -*- coding: utf-8
2// vim: set fileencoding=utf-8
3
4// This file is part of Eigen, a lightweight C++ template library
5// for linear algebra.
6//
7// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
8//
9// This Source Code Form is subject to the terms of the Mozilla
10// Public License v. 2.0. If a copy of the MPL was not distributed
11// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
12
13#ifndef EIGEN_HYBRIDNONLINEARSOLVER_H
14#define EIGEN_HYBRIDNONLINEARSOLVER_H
15
16// IWYU pragma: private
17#include "./InternalHeaderCheck.h"
18
19namespace Eigen {
20
21namespace HybridNonLinearSolverSpace {
22enum Status {
23 Running = -1,
24 ImproperInputParameters = 0,
25 RelativeErrorTooSmall = 1,
26 TooManyFunctionEvaluation = 2,
27 TolTooSmall = 3,
28 NotMakingProgressJacobian = 4,
29 NotMakingProgressIterations = 5,
30 UserAsked = 6
31};
32}
33
45template <typename FunctorType, typename Scalar = double>
46class HybridNonLinearSolver {
47 public:
48 typedef DenseIndex Index;
49
50 HybridNonLinearSolver(FunctorType &_functor) : functor(_functor) {
51 nfev = njev = iter = 0;
52 fnorm = 0.;
53 useExternalScaling = false;
54 }
55
56 struct Parameters {
57 Parameters()
58 : factor(Scalar(100.)),
59 maxfev(1000),
60 xtol(numext::sqrt(NumTraits<Scalar>::epsilon())),
61 nb_of_subdiagonals(-1),
62 nb_of_superdiagonals(-1),
63 epsfcn(Scalar(0.)) {}
64 Scalar factor;
65 Index maxfev; // maximum number of function evaluation
66 Scalar xtol;
67 Index nb_of_subdiagonals;
68 Index nb_of_superdiagonals;
69 Scalar epsfcn;
70 };
71 typedef Matrix<Scalar, Dynamic, 1> FVectorType;
72 typedef Matrix<Scalar, Dynamic, Dynamic> JacobianType;
73 /* TODO: if eigen provides a triangular storage, use it here */
74 typedef Matrix<Scalar, Dynamic, Dynamic> UpperTriangularType;
75
76 HybridNonLinearSolverSpace::Status hybrj1(FVectorType &x,
77 const Scalar tol = numext::sqrt(NumTraits<Scalar>::epsilon()));
78
79 HybridNonLinearSolverSpace::Status solveInit(FVectorType &x);
80 HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x);
81 HybridNonLinearSolverSpace::Status solve(FVectorType &x);
82
83 HybridNonLinearSolverSpace::Status hybrd1(FVectorType &x,
84 const Scalar tol = numext::sqrt(NumTraits<Scalar>::epsilon()));
85
86 HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x);
87 HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x);
88 HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x);
89
90 void resetParameters(void) { parameters = Parameters(); }
91 Parameters parameters;
92 FVectorType fvec, qtf, diag;
93 JacobianType fjac;
94 UpperTriangularType R;
95 Index nfev;
96 Index njev;
97 Index iter;
98 Scalar fnorm;
99 bool useExternalScaling;
100
101 private:
102 FunctorType &functor;
103 Index n;
104 Scalar sum;
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 &);
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 = 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 = 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 = 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>(n, n, fjac.data(), v_givens, w_givens);
310 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
311
312 jeval = false;
313 }
314 return HybridNonLinearSolverSpace::Running;
315}
316
317template <typename FunctorType, typename Scalar>
318HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solve(FVectorType &x) {
319 HybridNonLinearSolverSpace::Status status = solveInit(x);
320 if (status == HybridNonLinearSolverSpace::ImproperInputParameters) return status;
321 while (status == HybridNonLinearSolverSpace::Running) status = solveOneStep(x);
322 return status;
323}
324
325template <typename FunctorType, typename Scalar>
326HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::hybrd1(FVectorType &x,
327 const Scalar tol) {
328 n = x.size();
329
330 /* check the input parameters for errors. */
331 if (n <= 0 || tol < 0.) return HybridNonLinearSolverSpace::ImproperInputParameters;
332
333 resetParameters();
334 parameters.maxfev = 200 * (n + 1);
335 parameters.xtol = tol;
336
337 diag.setConstant(n, 1.);
338 useExternalScaling = true;
339 return solveNumericalDiff(x);
340}
341
342template <typename FunctorType, typename Scalar>
343HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solveNumericalDiffInit(FVectorType &x) {
344 n = x.size();
345
346 if (parameters.nb_of_subdiagonals < 0) parameters.nb_of_subdiagonals = n - 1;
347 if (parameters.nb_of_superdiagonals < 0) parameters.nb_of_superdiagonals = n - 1;
348
349 wa1.resize(n);
350 wa2.resize(n);
351 wa3.resize(n);
352 wa4.resize(n);
353 qtf.resize(n);
354 fjac.resize(n, n);
355 fvec.resize(n);
356 if (!useExternalScaling) diag.resize(n);
357 eigen_assert((!useExternalScaling || diag.size() == n) &&
358 "When useExternalScaling is set, the caller must provide a valid 'diag'");
359
360 /* Function Body */
361 nfev = 0;
362 njev = 0;
363
364 /* check the input parameters for errors. */
365 if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals < 0 ||
366 parameters.nb_of_superdiagonals < 0 || parameters.factor <= 0.)
367 return HybridNonLinearSolverSpace::ImproperInputParameters;
368 if (useExternalScaling)
369 for (Index j = 0; j < n; ++j)
370 if (diag[j] <= 0.) return HybridNonLinearSolverSpace::ImproperInputParameters;
371
372 /* evaluate the function at the starting point */
373 /* and calculate its norm. */
374 nfev = 1;
375 if (functor(x, fvec) < 0) return HybridNonLinearSolverSpace::UserAsked;
376 fnorm = fvec.stableNorm();
377
378 /* initialize iteration counter and monitors. */
379 iter = 1;
380 ncsuc = 0;
381 ncfail = 0;
382 nslow1 = 0;
383 nslow2 = 0;
384
385 return HybridNonLinearSolverSpace::Running;
386}
387
388template <typename FunctorType, typename Scalar>
389HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solveNumericalDiffOneStep(
390 FVectorType &x) {
391 using std::abs;
392 using std::sqrt;
393
394 eigen_assert(x.size() == n); // check the caller is not cheating us
395
396 Index j;
397 std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
398
399 jeval = true;
400 if (parameters.nb_of_subdiagonals < 0) parameters.nb_of_subdiagonals = n - 1;
401 if (parameters.nb_of_superdiagonals < 0) parameters.nb_of_superdiagonals = n - 1;
402
403 /* calculate the jacobian matrix. */
404 if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals,
405 parameters.epsfcn) < 0)
406 return HybridNonLinearSolverSpace::UserAsked;
407 nfev += (std::min)(parameters.nb_of_subdiagonals + parameters.nb_of_superdiagonals + 1, n);
408
409 wa2 = fjac.colwise().blueNorm();
410
411 /* on the first iteration and if external scaling is not used, scale according */
412 /* to the norms of the columns of the initial jacobian. */
413 if (iter == 1) {
414 if (!useExternalScaling)
415 for (j = 0; j < n; ++j) diag[j] = (wa2[j] == 0.) ? 1. : wa2[j];
416
417 /* on the first iteration, calculate the norm of the scaled x */
418 /* and initialize the step bound delta. */
419 xnorm = diag.cwiseProduct(x).stableNorm();
420 delta = parameters.factor * xnorm;
421 if (delta == 0.) delta = parameters.factor;
422 }
423
424 /* compute the qr factorization of the jacobian. */
425 HouseholderQR<JacobianType> qrfac(fjac); // no pivoting:
426
427 /* copy the triangular factor of the qr factorization into r. */
428 R = qrfac.matrixQR();
429
430 /* accumulate the orthogonal factor in fjac. */
431 fjac = qrfac.householderQ();
432
433 /* form (q transpose)*fvec and store in qtf. */
434 qtf = fjac.transpose() * fvec;
435
436 /* rescale if necessary. */
437 if (!useExternalScaling) diag = diag.cwiseMax(wa2);
438
439 while (true) {
440 /* determine the direction p. */
441 internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
442
443 /* store the direction p and x + p. calculate the norm of p. */
444 wa1 = -wa1;
445 wa2 = x + wa1;
446 pnorm = diag.cwiseProduct(wa1).stableNorm();
447
448 /* on the first iteration, adjust the initial step bound. */
449 if (iter == 1) delta = (std::min)(delta, pnorm);
450
451 /* evaluate the function at x + p and calculate its norm. */
452 if (functor(wa2, wa4) < 0) return HybridNonLinearSolverSpace::UserAsked;
453 ++nfev;
454 fnorm1 = wa4.stableNorm();
455
456 /* compute the scaled actual reduction. */
457 actred = -1.;
458 if (fnorm1 < fnorm) /* Computing 2nd power */
459 actred = 1. - numext::abs2(fnorm1 / fnorm);
460
461 /* compute the scaled predicted reduction. */
462 wa3 = R.template triangularView<Upper>() * wa1 + qtf;
463 temp = wa3.stableNorm();
464 prered = 0.;
465 if (temp < fnorm) /* Computing 2nd power */
466 prered = 1. - numext::abs2(temp / fnorm);
467
468 /* compute the ratio of the actual to the predicted reduction. */
469 ratio = 0.;
470 if (prered > 0.) ratio = actred / prered;
471
472 /* update the step bound. */
473 if (ratio < Scalar(.1)) {
474 ncsuc = 0;
475 ++ncfail;
476 delta = Scalar(.5) * delta;
477 } else {
478 ncfail = 0;
479 ++ncsuc;
480 if (ratio >= Scalar(.5) || ncsuc > 1) delta = (std::max)(delta, pnorm / Scalar(.5));
481 if (abs(ratio - 1.) <= Scalar(.1)) {
482 delta = pnorm / Scalar(.5);
483 }
484 }
485
486 /* test for successful iteration. */
487 if (ratio >= Scalar(1e-4)) {
488 /* successful iteration. update x, fvec, and their norms. */
489 x = wa2;
490 wa2 = diag.cwiseProduct(x);
491 fvec = wa4;
492 xnorm = wa2.stableNorm();
493 fnorm = fnorm1;
494 ++iter;
495 }
496
497 /* determine the progress of the iteration. */
498 ++nslow1;
499 if (actred >= Scalar(.001)) nslow1 = 0;
500 if (jeval) ++nslow2;
501 if (actred >= Scalar(.1)) nslow2 = 0;
502
503 /* test for convergence. */
504 if (delta <= parameters.xtol * xnorm || fnorm == 0.) return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
505
506 /* tests for termination and stringent tolerances. */
507 if (nfev >= parameters.maxfev) return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
508 if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
509 return HybridNonLinearSolverSpace::TolTooSmall;
510 if (nslow2 == 5) return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
511 if (nslow1 == 10) return HybridNonLinearSolverSpace::NotMakingProgressIterations;
512
513 /* criterion for recalculating jacobian. */
514 if (ncfail == 2) break; // leave inner loop and go for the next outer loop iteration
515
516 /* calculate the rank one modification to the jacobian */
517 /* and update qtf if necessary. */
518 wa1 = diag.cwiseProduct(diag.cwiseProduct(wa1) / pnorm);
519 wa2 = fjac.transpose() * wa4;
520 if (ratio >= Scalar(1e-4)) qtf = wa2;
521 wa2 = (wa2 - wa3) / pnorm;
522
523 /* compute the qr factorization of the updated jacobian. */
524 internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
525 internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
526 internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
527
528 jeval = false;
529 }
530 return HybridNonLinearSolverSpace::Running;
531}
532
533template <typename FunctorType, typename Scalar>
534HybridNonLinearSolverSpace::Status HybridNonLinearSolver<FunctorType, Scalar>::solveNumericalDiff(FVectorType &x) {
535 HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x);
536 if (status == HybridNonLinearSolverSpace::ImproperInputParameters) return status;
537 while (status == HybridNonLinearSolverSpace::Running) status = solveNumericalDiffOneStep(x);
538 return status;
539}
540
541} // end namespace Eigen
542
543#endif // EIGEN_HYBRIDNONLINEARSOLVER_H
544
545// vim: ai ts=4 sts=4 et sw=4
Eigen
Namespace containing all symbols from the Eigen library.
Eigen::abs
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_abs_op< typename Derived::Scalar >, const Derived > abs(const Eigen::ArrayBase< Derived > &x)
Eigen::Index
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index