diff options
Diffstat (limited to 'eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h')
| -rw-r--r-- | eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h | 377 |
1 files changed, 377 insertions, 0 deletions
diff --git a/eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h b/eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h new file mode 100644 index 0000000..51dd1d3 --- /dev/null +++ b/eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h @@ -0,0 +1,377 @@ +// This file is part of Eigen, a lightweight C++ template library +// for linear algebra. +// +// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> +// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr> +// +// The algorithm of this class initially comes from MINPACK whose original authors are: +// Copyright Jorge More - Argonne National Laboratory +// Copyright Burt Garbow - Argonne National Laboratory +// Copyright Ken Hillstrom - Argonne National Laboratory +// +// This Source Code Form is subject to the terms of the Minpack license +// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. +// +// 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_LEVENBERGMARQUARDT_H +#define EIGEN_LEVENBERGMARQUARDT_H + + +namespace Eigen { +namespace LevenbergMarquardtSpace { + enum Status { + NotStarted = -2, + Running = -1, + ImproperInputParameters = 0, + RelativeReductionTooSmall = 1, + RelativeErrorTooSmall = 2, + RelativeErrorAndReductionTooSmall = 3, + CosinusTooSmall = 4, + TooManyFunctionEvaluation = 5, + FtolTooSmall = 6, + XtolTooSmall = 7, + GtolTooSmall = 8, + UserAsked = 9 + }; +} + +template <typename _Scalar, int NX=Dynamic, int NY=Dynamic> +struct DenseFunctor +{ + typedef _Scalar Scalar; + enum { + InputsAtCompileTime = NX, + ValuesAtCompileTime = NY + }; + typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; + typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; + typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; + typedef ColPivHouseholderQR<JacobianType> QRSolver; + const int m_inputs, m_values; + + DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} + DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} + + int inputs() const { return m_inputs; } + int values() const { return m_values; } + + //int operator()(const InputType &x, ValueType& fvec) { } + // should be defined in derived classes + + //int df(const InputType &x, JacobianType& fjac) { } + // should be defined in derived classes +}; + +template <typename _Scalar, typename _Index> +struct SparseFunctor +{ + typedef _Scalar Scalar; + typedef _Index Index; + typedef Matrix<Scalar,Dynamic,1> InputType; + typedef Matrix<Scalar,Dynamic,1> ValueType; + typedef SparseMatrix<Scalar, ColMajor, Index> JacobianType; + typedef SparseQR<JacobianType, COLAMDOrdering<int> > QRSolver; + enum { + InputsAtCompileTime = Dynamic, + ValuesAtCompileTime = Dynamic + }; + + SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} + + int inputs() const { return m_inputs; } + int values() const { return m_values; } + + const int m_inputs, m_values; + //int operator()(const InputType &x, ValueType& fvec) { } + // to be defined in the functor + + //int df(const InputType &x, JacobianType& fjac) { } + // to be defined in the functor if no automatic differentiation + +}; +namespace internal { +template <typename QRSolver, typename VectorType> +void lmpar2(const QRSolver &qr, const VectorType &diag, const VectorType &qtb, + typename VectorType::Scalar m_delta, typename VectorType::Scalar &par, + VectorType &x); + } +/** + * \ingroup NonLinearOptimization_Module + * \brief Performs non linear optimization over a non-linear function, + * using a variant of the Levenberg Marquardt algorithm. + * + * Check wikipedia for more information. + * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm + */ +template<typename _FunctorType> +class LevenbergMarquardt : internal::no_assignment_operator +{ + public: + typedef _FunctorType FunctorType; + typedef typename FunctorType::QRSolver QRSolver; + typedef typename FunctorType::JacobianType JacobianType; + typedef typename JacobianType::Scalar Scalar; + typedef typename JacobianType::RealScalar RealScalar; + typedef typename JacobianType::Index Index; + typedef typename QRSolver::Index PermIndex; + typedef Matrix<Scalar,Dynamic,1> FVectorType; + typedef PermutationMatrix<Dynamic,Dynamic> PermutationType; + public: + LevenbergMarquardt(FunctorType& functor) + : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0), + m_isInitialized(false),m_info(InvalidInput) + { + resetParameters(); + m_useExternalScaling=false; + } + + LevenbergMarquardtSpace::Status minimize(FVectorType &x); + LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x); + LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x); + LevenbergMarquardtSpace::Status lmder1( + FVectorType &x, + const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) + ); + static LevenbergMarquardtSpace::Status lmdif1( + FunctorType &functor, + FVectorType &x, + Index *nfev, + const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) + ); + + /** Sets the default parameters */ + void resetParameters() + { + m_factor = 100.; + m_maxfev = 400; + m_ftol = std::sqrt(NumTraits<RealScalar>::epsilon()); + m_xtol = std::sqrt(NumTraits<RealScalar>::epsilon()); + m_gtol = 0. ; + m_epsfcn = 0. ; + } + + /** Sets the tolerance for the norm of the solution vector*/ + void setXtol(RealScalar xtol) { m_xtol = xtol; } + + /** Sets the tolerance for the norm of the vector function*/ + void setFtol(RealScalar ftol) { m_ftol = ftol; } + + /** Sets the tolerance for the norm of the gradient of the error vector*/ + void setGtol(RealScalar gtol) { m_gtol = gtol; } + + /** Sets the step bound for the diagonal shift */ + void setFactor(RealScalar factor) { m_factor = factor; } + + /** Sets the error precision */ + void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; } + + /** Sets the maximum number of function evaluation */ + void setMaxfev(Index maxfev) {m_maxfev = maxfev; } + + /** Use an external Scaling. If set to true, pass a nonzero diagonal to diag() */ + void setExternalScaling(bool value) {m_useExternalScaling = value; } + + /** \returns a reference to the diagonal of the jacobian */ + FVectorType& diag() {return m_diag; } + + /** \returns the number of iterations performed */ + Index iterations() { return m_iter; } + + /** \returns the number of functions evaluation */ + Index nfev() { return m_nfev; } + + /** \returns the number of jacobian evaluation */ + Index njev() { return m_njev; } + + /** \returns the norm of current vector function */ + RealScalar fnorm() {return m_fnorm; } + + /** \returns the norm of the gradient of the error */ + RealScalar gnorm() {return m_gnorm; } + + /** \returns the LevenbergMarquardt parameter */ + RealScalar lm_param(void) { return m_par; } + + /** \returns a reference to the current vector function + */ + FVectorType& fvec() {return m_fvec; } + + /** \returns a reference to the matrix where the current Jacobian matrix is stored + */ + JacobianType& jacobian() {return m_fjac; } + + /** \returns a reference to the triangular matrix R from the QR of the jacobian matrix. + * \sa jacobian() + */ + JacobianType& matrixR() {return m_rfactor; } + + /** the permutation used in the QR factorization + */ + PermutationType permutation() {return m_permutation; } + + /** + * \brief Reports whether the minimization was successful + * \returns \c Success if the minimization was succesful, + * \c NumericalIssue if a numerical problem arises during the + * minimization process, for exemple during the QR factorization + * \c NoConvergence if the minimization did not converge after + * the maximum number of function evaluation allowed + * \c InvalidInput if the input matrix is invalid + */ + ComputationInfo info() const + { + + return m_info; + } + private: + JacobianType m_fjac; + JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac + FunctorType &m_functor; + FVectorType m_fvec, m_qtf, m_diag; + Index n; + Index m; + Index m_nfev; + Index m_njev; + RealScalar m_fnorm; // Norm of the current vector function + RealScalar m_gnorm; //Norm of the gradient of the error + RealScalar m_factor; // + Index m_maxfev; // Maximum number of function evaluation + RealScalar m_ftol; //Tolerance in the norm of the vector function + RealScalar m_xtol; // + RealScalar m_gtol; //tolerance of the norm of the error gradient + RealScalar m_epsfcn; // + Index m_iter; // Number of iterations performed + RealScalar m_delta; + bool m_useExternalScaling; + PermutationType m_permutation; + FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors + RealScalar m_par; + bool m_isInitialized; // Check whether the minimization step has been called + ComputationInfo m_info; +}; + +template<typename FunctorType> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType>::minimize(FVectorType &x) +{ + LevenbergMarquardtSpace::Status status = minimizeInit(x); + if (status==LevenbergMarquardtSpace::ImproperInputParameters) { + m_isInitialized = true; + return status; + } + do { +// std::cout << " uv " << x.transpose() << "\n"; + status = minimizeOneStep(x); + } while (status==LevenbergMarquardtSpace::Running); + m_isInitialized = true; + return status; +} + +template<typename FunctorType> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType &x) +{ + n = x.size(); + m = m_functor.values(); + + m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n); + m_wa4.resize(m); + m_fvec.resize(m); + //FIXME Sparse Case : Allocate space for the jacobian + m_fjac.resize(m, n); +// m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative + if (!m_useExternalScaling) + m_diag.resize(n); + eigen_assert( (!m_useExternalScaling || m_diag.size()==n) || "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'"); + m_qtf.resize(n); + + /* Function Body */ + m_nfev = 0; + m_njev = 0; + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){ + m_info = InvalidInput; + return LevenbergMarquardtSpace::ImproperInputParameters; + } + + if (m_useExternalScaling) + for (Index j = 0; j < n; ++j) + if (m_diag[j] <= 0.) + { + m_info = InvalidInput; + return LevenbergMarquardtSpace::ImproperInputParameters; + } + + /* evaluate the function at the starting point */ + /* and calculate its norm. */ + m_nfev = 1; + if ( m_functor(x, m_fvec) < 0) + return LevenbergMarquardtSpace::UserAsked; + m_fnorm = m_fvec.stableNorm(); + + /* initialize levenberg-marquardt parameter and iteration counter. */ + m_par = 0.; + m_iter = 1; + + return LevenbergMarquardtSpace::NotStarted; +} + +template<typename FunctorType> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType>::lmder1( + FVectorType &x, + const Scalar tol + ) +{ + n = x.size(); + m = m_functor.values(); + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || tol < 0.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + resetParameters(); + m_ftol = tol; + m_xtol = tol; + m_maxfev = 100*(n+1); + + return minimize(x); +} + + +template<typename FunctorType> +LevenbergMarquardtSpace::Status +LevenbergMarquardt<FunctorType>::lmdif1( + FunctorType &functor, + FVectorType &x, + Index *nfev, + const Scalar tol + ) +{ + Index n = x.size(); + Index m = functor.values(); + + /* check the input parameters for errors. */ + if (n <= 0 || m < n || tol < 0.) + return LevenbergMarquardtSpace::ImproperInputParameters; + + NumericalDiff<FunctorType> numDiff(functor); + // embedded LevenbergMarquardt + LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff); + lm.setFtol(tol); + lm.setXtol(tol); + lm.setMaxfev(200*(n+1)); + + LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x)); + if (nfev) + * nfev = lm.nfev(); + return info; +} + +} // end namespace Eigen + +#endif // EIGEN_LEVENBERGMARQUARDT_H |