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