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(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 };
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 = 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
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 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 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
solve(FVectorType & x)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
hybrd1(FVectorType & x,const Scalar tol)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
solveNumericalDiffInit(FVectorType & x)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
solveNumericalDiffOneStep(FVectorType & x)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
solveNumericalDiff(FVectorType & x)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
597