diff options
Diffstat (limited to 'eigen/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h')
| -rw-r--r-- | eigen/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h | 601 |
1 files changed, 601 insertions, 0 deletions
diff --git a/eigen/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h b/eigen/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h new file mode 100644 index 0000000..b8ba6dd --- /dev/null +++ b/eigen/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h @@ -0,0 +1,601 @@ +// -*- coding: utf-8 +// vim: set fileencoding=utf-8 + +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef EIGEN_HYBRIDNONLINEARSOLVER_H +#define EIGEN_HYBRIDNONLINEARSOLVER_H + +namespace Eigen { + +namespace HybridNonLinearSolverSpace { + enum Status { + Running = -1, + ImproperInputParameters = 0, + RelativeErrorTooSmall = 1, + TooManyFunctionEvaluation = 2, + TolTooSmall = 3, + NotMakingProgressJacobian = 4, + NotMakingProgressIterations = 5, + UserAsked = 6 + }; +} + +/** + * \ingroup NonLinearOptimization_Module + * \brief Finds a zero of a system of n + * nonlinear functions in n variables by a modification of the Powell + * hybrid method ("dogleg"). + * + * The user must provide a subroutine which calculates the + * functions. The Jacobian is either provided by the user, or approximated + * using a forward-difference method. + * + */ +template<typename FunctorType, typename Scalar=double> +class HybridNonLinearSolver +{ +public: + typedef DenseIndex Index; + + HybridNonLinearSolver(FunctorType &_functor) + : functor(_functor) { nfev=njev=iter = 0; fnorm= 0.; useExternalScaling=false;} + + struct Parameters { + Parameters() + : factor(Scalar(100.)) + , maxfev(1000) + , xtol(std::sqrt(NumTraits<Scalar>::epsilon())) + , nb_of_subdiagonals(-1) + , nb_of_superdiagonals(-1) + , epsfcn(Scalar(0.)) {} + Scalar factor; + Index maxfev; // maximum number of function evaluation + Scalar xtol; + Index nb_of_subdiagonals; + Index nb_of_superdiagonals; + Scalar epsfcn; + }; + typedef Matrix< Scalar, Dynamic, 1 > FVectorType; + typedef Matrix< Scalar, Dynamic, Dynamic > JacobianType; + /* TODO: if eigen provides a triangular storage, use it here */ + typedef Matrix< Scalar, Dynamic, Dynamic > UpperTriangularType; + + HybridNonLinearSolverSpace::Status hybrj1( + FVectorType &x, + const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) + ); + + HybridNonLinearSolverSpace::Status solveInit(FVectorType &x); + HybridNonLinearSolverSpace::Status solveOneStep(FVectorType &x); + HybridNonLinearSolverSpace::Status solve(FVectorType &x); + + HybridNonLinearSolverSpace::Status hybrd1( + FVectorType &x, + const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) + ); + + HybridNonLinearSolverSpace::Status solveNumericalDiffInit(FVectorType &x); + HybridNonLinearSolverSpace::Status solveNumericalDiffOneStep(FVectorType &x); + HybridNonLinearSolverSpace::Status solveNumericalDiff(FVectorType &x); + + void resetParameters(void) { parameters = Parameters(); } + Parameters parameters; + FVectorType fvec, qtf, diag; + JacobianType fjac; + UpperTriangularType R; + Index nfev; + Index njev; + Index iter; + Scalar fnorm; + bool useExternalScaling; +private: + FunctorType &functor; + Index n; + Scalar sum; + bool sing; + Scalar temp; + Scalar delta; + bool jeval; + Index ncsuc; + Scalar ratio; + Scalar pnorm, xnorm, fnorm1; + Index nslow1, nslow2; + Index ncfail; + Scalar actred, prered; + FVectorType wa1, wa2, wa3, wa4; + + HybridNonLinearSolver& operator=(const HybridNonLinearSolver&); +}; + + + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::hybrj1( + FVectorType &x, + const Scalar tol + ) +{ + n = x.size(); + + /* check the input parameters for errors. */ + if (n <= 0 || tol < 0.) + return HybridNonLinearSolverSpace::ImproperInputParameters; + + resetParameters(); + parameters.maxfev = 100*(n+1); + parameters.xtol = tol; + diag.setConstant(n, 1.); + useExternalScaling = true; + return solve(x); +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveInit(FVectorType &x) +{ + n = x.size(); + + wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n); + fvec.resize(n); + qtf.resize(n); + fjac.resize(n, n); + if (!useExternalScaling) + diag.resize(n); + eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); + + /* Function Body */ + nfev = 0; + njev = 0; + + /* check the input parameters for errors. */ + if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.factor <= 0. ) + return HybridNonLinearSolverSpace::ImproperInputParameters; + if (useExternalScaling) + for (Index j = 0; j < n; ++j) + if (diag[j] <= 0.) + return HybridNonLinearSolverSpace::ImproperInputParameters; + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + nfev = 1; + if ( functor(x, fvec) < 0) + return HybridNonLinearSolverSpace::UserAsked; + fnorm = fvec.stableNorm(); + + /* initialize iteration counter and monitors. */ + iter = 1; + ncsuc = 0; + ncfail = 0; + nslow1 = 0; + nslow2 = 0; + + return HybridNonLinearSolverSpace::Running; +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveOneStep(FVectorType &x) +{ + using std::abs; + + eigen_assert(x.size()==n); // check the caller is not cheating us + + Index j; + std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); + + jeval = true; + + /* calculate the jacobian matrix. */ + if ( functor.df(x, fjac) < 0) + return HybridNonLinearSolverSpace::UserAsked; + ++njev; + + wa2 = fjac.colwise().blueNorm(); + + /* on the first iteration and if external scaling is not used, scale according */ + /* to the norms of the columns of the initial jacobian. */ + if (iter == 1) { + if (!useExternalScaling) + for (j = 0; j < n; ++j) + diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; + + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ + xnorm = diag.cwiseProduct(x).stableNorm(); + delta = parameters.factor * xnorm; + if (delta == 0.) + delta = parameters.factor; + } + + /* compute the qr factorization of the jacobian. */ + HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: + + /* copy the triangular factor of the qr factorization into r. */ + R = qrfac.matrixQR(); + + /* accumulate the orthogonal factor in fjac. */ + fjac = qrfac.householderQ(); + + /* form (q transpose)*fvec and store in qtf. */ + qtf = fjac.transpose() * fvec; + + /* rescale if necessary. */ + if (!useExternalScaling) + diag = diag.cwiseMax(wa2); + + while (true) { + /* determine the direction p. */ + internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); + + /* store the direction p and x + p. calculate the norm of p. */ + wa1 = -wa1; + wa2 = x + wa1; + pnorm = diag.cwiseProduct(wa1).stableNorm(); + + /* on the first iteration, adjust the initial step bound. */ + if (iter == 1) + delta = (std::min)(delta,pnorm); + + /* evaluate the function at x + p and calculate its norm. */ + if ( functor(wa2, wa4) < 0) + return HybridNonLinearSolverSpace::UserAsked; + ++nfev; + fnorm1 = wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + actred = -1.; + if (fnorm1 < fnorm) /* Computing 2nd power */ + actred = 1. - numext::abs2(fnorm1 / fnorm); + + /* compute the scaled predicted reduction. */ + wa3 = R.template triangularView<Upper>()*wa1 + qtf; + temp = wa3.stableNorm(); + prered = 0.; + if (temp < fnorm) /* Computing 2nd power */ + prered = 1. - numext::abs2(temp / fnorm); + + /* compute the ratio of the actual to the predicted reduction. */ + ratio = 0.; + if (prered > 0.) + ratio = actred / prered; + + /* update the step bound. */ + if (ratio < Scalar(.1)) { + ncsuc = 0; + ++ncfail; + delta = Scalar(.5) * delta; + } else { + ncfail = 0; + ++ncsuc; + if (ratio >= Scalar(.5) || ncsuc > 1) + delta = (std::max)(delta, pnorm / Scalar(.5)); + if (abs(ratio - 1.) <= Scalar(.1)) { + delta = pnorm / Scalar(.5); + } + } + + /* test for successful iteration. */ + if (ratio >= Scalar(1e-4)) { + /* successful iteration. update x, fvec, and their norms. */ + x = wa2; + wa2 = diag.cwiseProduct(x); + fvec = wa4; + xnorm = wa2.stableNorm(); + fnorm = fnorm1; + ++iter; + } + + /* determine the progress of the iteration. */ + ++nslow1; + if (actred >= Scalar(.001)) + nslow1 = 0; + if (jeval) + ++nslow2; + if (actred >= Scalar(.1)) + nslow2 = 0; + + /* test for convergence. */ + if (delta <= parameters.xtol * xnorm || fnorm == 0.) + return HybridNonLinearSolverSpace::RelativeErrorTooSmall; + + /* tests for termination and stringent tolerances. */ + if (nfev >= parameters.maxfev) + return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; + if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) + return HybridNonLinearSolverSpace::TolTooSmall; + if (nslow2 == 5) + return HybridNonLinearSolverSpace::NotMakingProgressJacobian; + if (nslow1 == 10) + return HybridNonLinearSolverSpace::NotMakingProgressIterations; + + /* criterion for recalculating jacobian. */ + if (ncfail == 2) + break; // leave inner loop and go for the next outer loop iteration + + /* calculate the rank one modification to the jacobian */ + /* and update qtf if necessary. */ + wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); + wa2 = fjac.transpose() * wa4; + if (ratio >= Scalar(1e-4)) + qtf = wa2; + wa2 = (wa2-wa3)/pnorm; + + /* compute the qr factorization of the updated jacobian. */ + internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); + internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); + internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); + + jeval = false; + } + return HybridNonLinearSolverSpace::Running; +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solve(FVectorType &x) +{ + HybridNonLinearSolverSpace::Status status = solveInit(x); + if (status==HybridNonLinearSolverSpace::ImproperInputParameters) + return status; + while (status==HybridNonLinearSolverSpace::Running) + status = solveOneStep(x); + return status; +} + + + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::hybrd1( + FVectorType &x, + const Scalar tol + ) +{ + n = x.size(); + + /* check the input parameters for errors. */ + if (n <= 0 || tol < 0.) + return HybridNonLinearSolverSpace::ImproperInputParameters; + + resetParameters(); + parameters.maxfev = 200*(n+1); + parameters.xtol = tol; + + diag.setConstant(n, 1.); + useExternalScaling = true; + return solveNumericalDiff(x); +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffInit(FVectorType &x) +{ + n = x.size(); + + if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; + if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; + + wa1.resize(n); wa2.resize(n); wa3.resize(n); wa4.resize(n); + qtf.resize(n); + fjac.resize(n, n); + fvec.resize(n); + if (!useExternalScaling) + diag.resize(n); + eigen_assert( (!useExternalScaling || diag.size()==n) || "When useExternalScaling is set, the caller must provide a valid 'diag'"); + + /* Function Body */ + nfev = 0; + njev = 0; + + /* check the input parameters for errors. */ + if (n <= 0 || parameters.xtol < 0. || parameters.maxfev <= 0 || parameters.nb_of_subdiagonals< 0 || parameters.nb_of_superdiagonals< 0 || parameters.factor <= 0. ) + return HybridNonLinearSolverSpace::ImproperInputParameters; + if (useExternalScaling) + for (Index j = 0; j < n; ++j) + if (diag[j] <= 0.) + return HybridNonLinearSolverSpace::ImproperInputParameters; + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + nfev = 1; + if ( functor(x, fvec) < 0) + return HybridNonLinearSolverSpace::UserAsked; + fnorm = fvec.stableNorm(); + + /* initialize iteration counter and monitors. */ + iter = 1; + ncsuc = 0; + ncfail = 0; + nslow1 = 0; + nslow2 = 0; + + return HybridNonLinearSolverSpace::Running; +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiffOneStep(FVectorType &x) +{ + using std::sqrt; + using std::abs; + + assert(x.size()==n); // check the caller is not cheating us + + Index j; + std::vector<JacobiRotation<Scalar> > v_givens(n), w_givens(n); + + jeval = true; + if (parameters.nb_of_subdiagonals<0) parameters.nb_of_subdiagonals= n-1; + if (parameters.nb_of_superdiagonals<0) parameters.nb_of_superdiagonals= n-1; + + /* calculate the jacobian matrix. */ + if (internal::fdjac1(functor, x, fvec, fjac, parameters.nb_of_subdiagonals, parameters.nb_of_superdiagonals, parameters.epsfcn) <0) + return HybridNonLinearSolverSpace::UserAsked; + nfev += (std::min)(parameters.nb_of_subdiagonals+parameters.nb_of_superdiagonals+ 1, n); + + wa2 = fjac.colwise().blueNorm(); + + /* on the first iteration and if external scaling is not used, scale according */ + /* to the norms of the columns of the initial jacobian. */ + if (iter == 1) { + if (!useExternalScaling) + for (j = 0; j < n; ++j) + diag[j] = (wa2[j]==0.) ? 1. : wa2[j]; + + /* on the first iteration, calculate the norm of the scaled x */ + /* and initialize the step bound delta. */ + xnorm = diag.cwiseProduct(x).stableNorm(); + delta = parameters.factor * xnorm; + if (delta == 0.) + delta = parameters.factor; + } + + /* compute the qr factorization of the jacobian. */ + HouseholderQR<JacobianType> qrfac(fjac); // no pivoting: + + /* copy the triangular factor of the qr factorization into r. */ + R = qrfac.matrixQR(); + + /* accumulate the orthogonal factor in fjac. */ + fjac = qrfac.householderQ(); + + /* form (q transpose)*fvec and store in qtf. */ + qtf = fjac.transpose() * fvec; + + /* rescale if necessary. */ + if (!useExternalScaling) + diag = diag.cwiseMax(wa2); + + while (true) { + /* determine the direction p. */ + internal::dogleg<Scalar>(R, diag, qtf, delta, wa1); + + /* store the direction p and x + p. calculate the norm of p. */ + wa1 = -wa1; + wa2 = x + wa1; + pnorm = diag.cwiseProduct(wa1).stableNorm(); + + /* on the first iteration, adjust the initial step bound. */ + if (iter == 1) + delta = (std::min)(delta,pnorm); + + /* evaluate the function at x + p and calculate its norm. */ + if ( functor(wa2, wa4) < 0) + return HybridNonLinearSolverSpace::UserAsked; + ++nfev; + fnorm1 = wa4.stableNorm(); + + /* compute the scaled actual reduction. */ + actred = -1.; + if (fnorm1 < fnorm) /* Computing 2nd power */ + actred = 1. - numext::abs2(fnorm1 / fnorm); + + /* compute the scaled predicted reduction. */ + wa3 = R.template triangularView<Upper>()*wa1 + qtf; + temp = wa3.stableNorm(); + prered = 0.; + if (temp < fnorm) /* Computing 2nd power */ + prered = 1. - numext::abs2(temp / fnorm); + + /* compute the ratio of the actual to the predicted reduction. */ + ratio = 0.; + if (prered > 0.) + ratio = actred / prered; + + /* update the step bound. */ + if (ratio < Scalar(.1)) { + ncsuc = 0; + ++ncfail; + delta = Scalar(.5) * delta; + } else { + ncfail = 0; + ++ncsuc; + if (ratio >= Scalar(.5) || ncsuc > 1) + delta = (std::max)(delta, pnorm / Scalar(.5)); + if (abs(ratio - 1.) <= Scalar(.1)) { + delta = pnorm / Scalar(.5); + } + } + + /* test for successful iteration. */ + if (ratio >= Scalar(1e-4)) { + /* successful iteration. update x, fvec, and their norms. */ + x = wa2; + wa2 = diag.cwiseProduct(x); + fvec = wa4; + xnorm = wa2.stableNorm(); + fnorm = fnorm1; + ++iter; + } + + /* determine the progress of the iteration. */ + ++nslow1; + if (actred >= Scalar(.001)) + nslow1 = 0; + if (jeval) + ++nslow2; + if (actred >= Scalar(.1)) + nslow2 = 0; + + /* test for convergence. */ + if (delta <= parameters.xtol * xnorm || fnorm == 0.) + return HybridNonLinearSolverSpace::RelativeErrorTooSmall; + + /* tests for termination and stringent tolerances. */ + if (nfev >= parameters.maxfev) + return HybridNonLinearSolverSpace::TooManyFunctionEvaluation; + if (Scalar(.1) * (std::max)(Scalar(.1) * delta, pnorm) <= NumTraits<Scalar>::epsilon() * xnorm) + return HybridNonLinearSolverSpace::TolTooSmall; + if (nslow2 == 5) + return HybridNonLinearSolverSpace::NotMakingProgressJacobian; + if (nslow1 == 10) + return HybridNonLinearSolverSpace::NotMakingProgressIterations; + + /* criterion for recalculating jacobian. */ + if (ncfail == 2) + break; // leave inner loop and go for the next outer loop iteration + + /* calculate the rank one modification to the jacobian */ + /* and update qtf if necessary. */ + wa1 = diag.cwiseProduct( diag.cwiseProduct(wa1)/pnorm ); + wa2 = fjac.transpose() * wa4; + if (ratio >= Scalar(1e-4)) + qtf = wa2; + wa2 = (wa2-wa3)/pnorm; + + /* compute the qr factorization of the updated jacobian. */ + internal::r1updt<Scalar>(R, wa1, v_givens, w_givens, wa2, wa3, &sing); + internal::r1mpyq<Scalar>(n, n, fjac.data(), v_givens, w_givens); + internal::r1mpyq<Scalar>(1, n, qtf.data(), v_givens, w_givens); + + jeval = false; + } + return HybridNonLinearSolverSpace::Running; +} + +template<typename FunctorType, typename Scalar> +HybridNonLinearSolverSpace::Status +HybridNonLinearSolver<FunctorType,Scalar>::solveNumericalDiff(FVectorType &x) +{ + HybridNonLinearSolverSpace::Status status = solveNumericalDiffInit(x); + if (status==HybridNonLinearSolverSpace::ImproperInputParameters) + return status; + while (status==HybridNonLinearSolverSpace::Running) + status = solveNumericalDiffOneStep(x); + return status; +} + +} // end namespace Eigen + +#endif // EIGEN_HYBRIDNONLINEARSOLVER_H + +//vim: ai ts=4 sts=4 et sw=4 |