HybridNonLinearSolver.h revision 7faaa9f3f0df9d23790277834d426c3d992ac3ba 
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
16namespace Eigen {
17
18namespace 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  */
42template<typename FunctorType, typename Scalar=double>
43class HybridNonLinearSolver
44{
45public:
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(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
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;
99private:
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
120template<typename FunctorType, typename Scalar>
121HybridNonLinearSolverSpace::Status
122HybridNonLinearSolver<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
141template<typename FunctorType, typename Scalar>
142HybridNonLinearSolverSpace::Status
143HybridNonLinearSolver<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
184template<typename FunctorType, typename Scalar>
185HybridNonLinearSolverSpace::Status
186HybridNonLinearSolver<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
342template<typename FunctorType, typename Scalar>
343HybridNonLinearSolverSpace::Status
344HybridNonLinearSolver<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
356template<typename FunctorType, typename Scalar>
357HybridNonLinearSolverSpace::Status
358HybridNonLinearSolver<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
378template<typename FunctorType, typename Scalar>
379HybridNonLinearSolverSpace::Status
380HybridNonLinearSolver<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
424template<typename FunctorType, typename Scalar>
425HybridNonLinearSolverSpace::Status
426HybridNonLinearSolver<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
585template<typename FunctorType, typename Scalar>
586HybridNonLinearSolverSpace::Status
587HybridNonLinearSolver<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