| // -*- 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(numext::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 = numext::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 = numext::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 |
