HybridNonLinearSolver.h
Go to the documentation of this file.
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,
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 {
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.)) {}
60  Index maxfev; // maximum number of function evaluation
65  };
68  /* TODO: if eigen provides a triangular storage, use it here */
70 
72  FVectorType &x,
74  );
75 
76  HybridNonLinearSolverSpace::Status solveInit(FVectorType &x);
77  HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x);
78  HybridNonLinearSolverSpace::Status solve(FVectorType &x);
79 
81  FVectorType &x,
83  );
84 
85  HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x);
86  HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x);
87  HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x);
88 
91  FVectorType fvec, qtf, diag;
92  JacobianType fjac;
93  UpperTriangularType R;
94  Index nfev;
95  Index njev;
96  Index iter;
99 private:
100  FunctorType &functor;
101  Index n;
103  bool sing;
106  bool jeval;
107  Index ncsuc;
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>
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.)
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>
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. )
162  if (useExternalScaling)
163  for (Index j = 0; j < n; ++j)
164  if (diag[j] <= 0.)
166 
167  /* evaluate the function at the starting point */
168  /* and calculate its norm. */
169  nfev = 1;
170  if ( functor(x, fvec) < 0)
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 
182 }
183 
184 template<typename FunctorType, typename Scalar>
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)
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)
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.)
309 
310  /* tests for termination and stringent tolerances. */
311  if (nfev >= parameters.maxfev)
313  if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
315  if (nslow2 == 5)
317  if (nslow1 == 10)
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  }
340 }
341 
342 template<typename FunctorType, typename Scalar>
345 {
346  HybridNonLinearSolverSpace::Status status = solveInit(x);
348  return status;
350  status = solveOneStep(x);
351  return status;
352 }
353 
354 
355 
356 template<typename FunctorType, typename Scalar>
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.)
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>
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. )
402  if (useExternalScaling)
403  for (Index j = 0; j < n; ++j)
404  if (diag[j] <= 0.)
406 
407  /* evaluate the function at the starting point */
408  /* and calculate its norm. */
409  nfev = 1;
410  if ( functor(x, fvec) < 0)
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 
422 }
423 
424 template<typename FunctorType, typename Scalar>
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)
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)
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.)
552 
553  /* tests for termination and stringent tolerances. */
554  if (nfev >= parameters.maxfev)
556  if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm)
558  if (nslow2 == 5)
560  if (nslow1 == 10)
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  }
583 }
584 
585 template<typename FunctorType, typename Scalar>
588 {
589  HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x);
591  return status;
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
Matrix< Scalar, Dynamic, Dynamic > UpperTriangularType
SCALAR Scalar
Definition: bench_gemm.cpp:33
HouseholderSequenceType householderQ() const
#define max(a, b)
Definition: datatypes.h:20
HybridNonLinearSolver(FunctorType &_functor)
Matrix diag(const std::vector< Matrix > &Hs)
Definition: Matrix.cpp:206
HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x)
#define min(a, b)
Definition: datatypes.h:19
int n
Rot2 R(Rot2::fromAngle(0.1))
EIGEN_DEVICE_FUNC const SqrtReturnType sqrt() const
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Definition: Half.h:150
iterator iter(handle obj)
Definition: pytypes.h:1547
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:150
HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x)
HybridNonLinearSolverSpace::Status solve(FVectorType &x)
static double epsilon
Definition: testRot3.cpp:39
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size ratio
HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x)
Matrix< Scalar, Dynamic, Dynamic > JacobianType
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void resize(Index rows, Index cols)
#define eigen_assert(x)
Definition: Macros.h:579
Array< double, 1, 3 > e(1./3., 0.5, 2.)
HybridNonLinearSolverSpace::Status hybrd1(FVectorType &x, const Scalar tol=std::sqrt(NumTraits< Scalar >::epsilon()))
HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x)
static ConjugateGradientParameters parameters
EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Abs2ReturnType abs2() const
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: Meta.h:25
HybridNonLinearSolverSpace::Status hybrj1(FVectorType &x, const Scalar tol=std::sqrt(NumTraits< Scalar >::epsilon()))
HybridNonLinearSolverSpace::Status solveInit(FVectorType &x)
Householder QR decomposition of a matrix.
const G double tol
Definition: Group.h:83
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
Matrix< Scalar, Dynamic, 1 > FVectorType
#define abs(x)
Definition: datatypes.h:17
Finds a zero of a system of n nonlinear functions in n variables by a modification of the Powell hybr...
std::ptrdiff_t j
const MatrixType & matrixQR() const
DenseIndex fdjac1(const FunctorType &Functor, Matrix< Scalar, Dynamic, 1 > &x, Matrix< Scalar, Dynamic, 1 > &fvec, Matrix< Scalar, Dynamic, Dynamic > &fjac, DenseIndex ml, DenseIndex mu, Scalar epsfcn)
Definition: fdjac1.h:6


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:42:12