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