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
31 /**
32 * \ingroup NonLinearOptimization_Module
33 * \brief Finds a zero of a system of n
34 * nonlinear functions in n variables by a modification of the Powell
35 * hybrid method ("dogleg").
36 *
37 * The user must provide a subroutine which calculates the
38 * functions. The Jacobian is either provided by the user, or approximated
39 * using a forward-difference method.
40 *
41 */
42 template<typename FunctorType, typename Scalar=double>
43 class HybridNonLinearSolver
44 {
45 public:
46 typedef DenseIndex Index;
47
HybridNonLinearSolver(FunctorType & _functor)48 HybridNonLinearSolver(FunctorType &_functor)
49 : functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; useExternalScaling=false;}
50
51 struct Parameters {
ParametersParameters52 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
resetParameters(void)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
hybrj1(FVectorType & x,const Scalar tol)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
solveInit(FVectorType & x)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
solveOneStep(FVectorType & x)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
solve(FVectorType & x)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
hybrd1(FVectorType & x,const Scalar tol)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
solveNumericalDiffInit(FVectorType & x)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
solveNumericalDiffOneStep(FVectorType & x)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
solveNumericalDiff(FVectorType & x)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
602