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 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>
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(internal::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  };
68  /* TODO: if eigen provides a triangular storage, use it here */
70 
71  HybridNonLinearSolverSpace::Status hybrj1(
72  FVectorType &x,
73  const Scalar tol = internal::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 = internal::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;
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
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  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  assert(x.size()==n); // check the caller is not cheating us
189 
190  Index j;
191  std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
192 
193  jeval = true;
194 
195  /* calculate the jacobian matrix. */
196  if ( functor.df(x, fjac) < 0)
197  return HybridNonLinearSolverSpace::UserAsked;
198  ++njev;
199 
200  wa2 = fjac.colwise().blueNorm();
201 
202  /* on the first iteration and if external scaling is not used, scale according */
203  /* to the norms of the columns of the initial jacobian. */
204  if (iter == 1) {
205  if (!useExternalScaling)
206  for (j = 0; j < n; ++j)
207  diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
208 
209  /* on the first iteration, calculate the norm of the scaled x */
210  /* and initialize the step bound delta. */
211  xnorm = diag.cwiseProduct(x).stableNorm();
212  delta = parameters.factor * xnorm;
213  if (delta == 0.)
214  delta = parameters.factor;
215  }
216 
217  /* compute the qr factorization of the jacobian. */
218  HouseholderQR<JacobianType> qrfac(fjac); // no pivoting:
219 
220  /* copy the triangular factor of the qr factorization into r. */
221  R = qrfac.matrixQR();
222 
223  /* accumulate the orthogonal factor in fjac. */
224  fjac = qrfac.householderQ();
225 
226  /* form (q transpose)*fvec and store in qtf. */
227  qtf = fjac.transpose() * fvec;
228 
229  /* rescale if necessary. */
230  if (!useExternalScaling)
231  diag = diag.cwiseMax(wa2);
232 
233  while (true) {
234  /* determine the direction p. */
235  internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
236 
237  /* store the direction p and x + p. calculate the norm of p. */
238  wa1 = -wa1;
239  wa2 = x + wa1;
240  pnorm = diag.cwiseProduct(wa1).stableNorm();
241 
242  /* on the first iteration, adjust the initial step bound. */
243  if (iter == 1)
244  delta = (std::min)(delta,pnorm);
245 
246  /* evaluate the function at x + p and calculate its norm. */
247  if ( functor(wa2, wa4) < 0)
248  return HybridNonLinearSolverSpace::UserAsked;
249  ++nfev;
250  fnorm1 = wa4.stableNorm();
251 
252  /* compute the scaled actual reduction. */
253  actred = -1.;
254  if (fnorm1 < fnorm) /* Computing 2nd power */
255  actred = 1. - internal::abs2(fnorm1 / fnorm);
256 
257  /* compute the scaled predicted reduction. */
258  wa3 = R.template triangularView<Upper>()*wa1 + qtf;
259  temp = wa3.stableNorm();
260  prered = 0.;
261  if (temp < fnorm) /* Computing 2nd power */
262  prered = 1. - internal::abs2(temp / fnorm);
263 
264  /* compute the ratio of the actual to the predicted reduction. */
265  ratio = 0.;
266  if (prered > 0.)
267  ratio = actred / prered;
268 
269  /* update the step bound. */
270  if (ratio < Scalar(.1)) {
271  ncsuc = 0;
272  ++ncfail;
273  delta = Scalar(.5) * delta;
274  } else {
275  ncfail = 0;
276  ++ncsuc;
277  if (ratio >= Scalar(.5) || ncsuc > 1)
278  delta = (std::max)(delta, pnorm / Scalar(.5));
279  if (internal::abs(ratio - 1.) <= Scalar(.1)) {
280  delta = pnorm / Scalar(.5);
281  }
282  }
283 
284  /* test for successful iteration. */
285  if (ratio >= Scalar(1e-4)) {
286  /* successful iteration. update x, fvec, and their norms. */
287  x = wa2;
288  wa2 = diag.cwiseProduct(x);
289  fvec = wa4;
290  xnorm = wa2.stableNorm();
291  fnorm = fnorm1;
292  ++iter;
293  }
294 
295  /* determine the progress of the iteration. */
296  ++nslow1;
297  if (actred >= Scalar(.001))
298  nslow1 = 0;
299  if (jeval)
300  ++nslow2;
301  if (actred >= Scalar(.1))
302  nslow2 = 0;
303 
304  /* test for convergence. */
305  if (delta <= parameters.xtol * xnorm || fnorm == 0.)
306  return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
307 
308  /* tests for termination and stringent tolerances. */
309  if (nfev >= parameters.maxfev)
310  return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
311  if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
312  return HybridNonLinearSolverSpace::TolTooSmall;
313  if (nslow2 == 5)
314  return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
315  if (nslow1 == 10)
316  return HybridNonLinearSolverSpace::NotMakingProgressIterations;
317 
318  /* criterion for recalculating jacobian. */
319  if (ncfail == 2)
320  break; // leave inner loop and go for the next outer loop iteration
321 
322  /* calculate the rank one modification to the jacobian */
323  /* and update qtf if necessary. */
324  wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
325  wa2 = fjac.transpose() * wa4;
326  if (ratio >= Scalar(1e-4))
327  qtf = wa2;
328  wa2 = (wa2-wa3)/pnorm;
329 
330  /* compute the qr factorization of the updated jacobian. */
331  internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
332  internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
333  internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
334 
335  jeval = false;
336  }
337  return HybridNonLinearSolverSpace::Running;
338 }
339 
340 template<typename FunctorType, typename Scalar>
341 HybridNonLinearSolverSpace::Status
342 HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x)
343 {
344  HybridNonLinearSolverSpace::Status status = solveInit(x);
345  if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
346  return status;
347  while (status==HybridNonLinearSolverSpace::Running)
348  status = solveOneStep(x);
349  return status;
350 }
351 
352 
353 
354 template<typename FunctorType, typename Scalar>
355 HybridNonLinearSolverSpace::Status
356 HybridNonLinearSolver<FunctorType,Scalar>::hybrd1(
357  FVectorType &x,
358  const Scalar tol
359  )
360 {
361  n = x.size();
362 
363  /* check the input parameters for errors. */
364  if (n <= 0 || tol < 0.)
365  return HybridNonLinearSolverSpace::ImproperInputParameters;
366 
367  resetParameters();
368  parameters.maxfev = 200*(n+1);
369  parameters.xtol = tol;
370 
371  diag.setConstant(n, 1.);
372  useExternalScaling = true;
373  return solveNumericalDiff(x);
374 }
375 
376 template<typename FunctorType, typename Scalar>
377 HybridNonLinearSolverSpace::Status
378 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x)
379 {
380  n = x.size();
381 
382  if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
383  if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
384 
385  wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n);
386  qtf.resize(n);
387  fjac.resize(n, n);
388  fvec.resize(n);
389  if (!useExternalScaling)
390  diag.resize(n);
391  assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'");
392 
393  /* Function Body */
394  nfev = 0;
395  njev = 0;
396 
397  /* check the input parameters for errors. */
398  if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. )
399  return HybridNonLinearSolverSpace::ImproperInputParameters;
400  if (useExternalScaling)
401  for (Index j = 0; j < n; ++j)
402  if (diag[j] <= 0.)
403  return HybridNonLinearSolverSpace::ImproperInputParameters;
404 
405  /* evaluate the function at the starting point */
406  /* and calculate its norm. */
407  nfev = 1;
408  if ( functor(x, fvec) < 0)
409  return HybridNonLinearSolverSpace::UserAsked;
410  fnorm = fvec.stableNorm();
411 
412  /* initialize iteration counter and monitors. */
413  iter = 1;
414  ncsuc = 0;
415  ncfail = 0;
416  nslow1 = 0;
417  nslow2 = 0;
418 
419  return HybridNonLinearSolverSpace::Running;
420 }
421 
422 template<typename FunctorType, typename Scalar>
423 HybridNonLinearSolverSpace::Status
424 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x)
425 {
426  assert(x.size()==n); // check the caller is not cheating us
427 
428  Index j;
429  std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n);
430 
431  jeval = true;
432  if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1;
433  if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1;
434 
435  /* calculate the jacobian matrix. */
436  if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0)
437  return HybridNonLinearSolverSpace::UserAsked;
438  nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n);
439 
440  wa2 = fjac.colwise().blueNorm();
441 
442  /* on the first iteration and if external scaling is not used, scale according */
443  /* to the norms of the columns of the initial jacobian. */
444  if (iter == 1) {
445  if (!useExternalScaling)
446  for (j = 0; j < n; ++j)
447  diag[j] = (wa2[j]==0.) ? 1. : wa2[j];
448 
449  /* on the first iteration, calculate the norm of the scaled x */
450  /* and initialize the step bound delta. */
451  xnorm = diag.cwiseProduct(x).stableNorm();
452  delta = parameters.factor * xnorm;
453  if (delta == 0.)
454  delta = parameters.factor;
455  }
456 
457  /* compute the qr factorization of the jacobian. */
458  HouseholderQR<JacobianType> qrfac(fjac); // no pivoting:
459 
460  /* copy the triangular factor of the qr factorization into r. */
461  R = qrfac.matrixQR();
462 
463  /* accumulate the orthogonal factor in fjac. */
464  fjac = qrfac.householderQ();
465 
466  /* form (q transpose)*fvec and store in qtf. */
467  qtf = fjac.transpose() * fvec;
468 
469  /* rescale if necessary. */
470  if (!useExternalScaling)
471  diag = diag.cwiseMax(wa2);
472 
473  while (true) {
474  /* determine the direction p. */
475  internal::dogleg<Scalar>(R, diag, qtf, delta, wa1);
476 
477  /* store the direction p and x + p. calculate the norm of p. */
478  wa1 = -wa1;
479  wa2 = x + wa1;
480  pnorm = diag.cwiseProduct(wa1).stableNorm();
481 
482  /* on the first iteration, adjust the initial step bound. */
483  if (iter == 1)
484  delta = (std::min)(delta,pnorm);
485 
486  /* evaluate the function at x + p and calculate its norm. */
487  if ( functor(wa2, wa4) < 0)
488  return HybridNonLinearSolverSpace::UserAsked;
489  ++nfev;
490  fnorm1 = wa4.stableNorm();
491 
492  /* compute the scaled actual reduction. */
493  actred = -1.;
494  if (fnorm1 < fnorm) /* Computing 2nd power */
495  actred = 1. - internal::abs2(fnorm1 / fnorm);
496 
497  /* compute the scaled predicted reduction. */
498  wa3 = R.template triangularView<Upper>()*wa1 + qtf;
499  temp = wa3.stableNorm();
500  prered = 0.;
501  if (temp < fnorm) /* Computing 2nd power */
502  prered = 1. - internal::abs2(temp / fnorm);
503 
504  /* compute the ratio of the actual to the predicted reduction. */
505  ratio = 0.;
506  if (prered > 0.)
507  ratio = actred / prered;
508 
509  /* update the step bound. */
510  if (ratio < Scalar(.1)) {
511  ncsuc = 0;
512  ++ncfail;
513  delta = Scalar(.5) * delta;
514  } else {
515  ncfail = 0;
516  ++ncsuc;
517  if (ratio >= Scalar(.5) || ncsuc > 1)
518  delta = (std::max)(delta, pnorm / Scalar(.5));
519  if (internal::abs(ratio - 1.) <= Scalar(.1)) {
520  delta = pnorm / Scalar(.5);
521  }
522  }
523 
524  /* test for successful iteration. */
525  if (ratio >= Scalar(1e-4)) {
526  /* successful iteration. update x, fvec, and their norms. */
527  x = wa2;
528  wa2 = diag.cwiseProduct(x);
529  fvec = wa4;
530  xnorm = wa2.stableNorm();
531  fnorm = fnorm1;
532  ++iter;
533  }
534 
535  /* determine the progress of the iteration. */
536  ++nslow1;
537  if (actred >= Scalar(.001))
538  nslow1 = 0;
539  if (jeval)
540  ++nslow2;
541  if (actred >= Scalar(.1))
542  nslow2 = 0;
543 
544  /* test for convergence. */
545  if (delta <= parameters.xtol * xnorm || fnorm == 0.)
546  return HybridNonLinearSolverSpace::RelativeErrorTooSmall;
547 
548  /* tests for termination and stringent tolerances. */
549  if (nfev >= parameters.maxfev)
550  return HybridNonLinearSolverSpace::TooManyFunctionEvaluation;
551  if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
552  return HybridNonLinearSolverSpace::TolTooSmall;
553  if (nslow2 == 5)
554  return HybridNonLinearSolverSpace::NotMakingProgressJacobian;
555  if (nslow1 == 10)
556  return HybridNonLinearSolverSpace::NotMakingProgressIterations;
557 
558  /* criterion for recalculating jacobian. */
559  if (ncfail == 2)
560  break; // leave inner loop and go for the next outer loop iteration
561 
562  /* calculate the rank one modification to the jacobian */
563  /* and update qtf if necessary. */
564  wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm );
565  wa2 = fjac.transpose() * wa4;
566  if (ratio >= Scalar(1e-4))
567  qtf = wa2;
568  wa2 = (wa2-wa3)/pnorm;
569 
570  /* compute the qr factorization of the updated jacobian. */
571  internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing);
572  internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens);
573  internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens);
574 
575  jeval = false;
576  }
577  return HybridNonLinearSolverSpace::Running;
578 }
579 
580 template<typename FunctorType, typename Scalar>
581 HybridNonLinearSolverSpace::Status
582 HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x)
583 {
584  HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x);
585  if (status==HybridNonLinearSolverSpace::ImproperInputParameters)
586  return status;
587  while (status==HybridNonLinearSolverSpace::Running)
588  status = solveNumericalDiffOneStep(x);
589  return status;
590 }
591 
592 } // end namespace Eigen
593 
594 #endif // EIGEN_HYBRIDNONLINEARSOLVER_H
595 
596 //vim: ai ts=4 sts=4 et sw=4