HybridNonLinearSolver.h revision c981c48f5bc9aefeffc0bcb0cc3934c2fae179dd 
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(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
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    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    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
340template<typename FunctorType, typename Scalar>
341HybridNonLinearSolverSpace::Status
342HybridNonLinearSolver<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
354template<typename FunctorType, typename Scalar>
355HybridNonLinearSolverSpace::Status
356HybridNonLinearSolver<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
376template<typename FunctorType, typename Scalar>
377HybridNonLinearSolverSpace::Status
378HybridNonLinearSolver<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
422template<typename FunctorType, typename Scalar>
423HybridNonLinearSolverSpace::Status
424HybridNonLinearSolver<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
580template<typename FunctorType, typename Scalar>
581HybridNonLinearSolverSpace::Status
582HybridNonLinearSolver<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