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