17faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This file is part of Eigen, a lightweight C++ template library 27faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// for linear algebra. 37faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 47faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> 57faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr> 67faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 77faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// The algorithm of this class initially comes from MINPACK whose original authors are: 87faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright Jorge More - Argonne National Laboratory 97faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright Burt Garbow - Argonne National Laboratory 107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Copyright Ken Hillstrom - Argonne National Laboratory 117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This Source Code Form is subject to the terms of the Minpack license 137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// 157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// This Source Code Form is subject to the terms of the Mozilla 167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// Public License v. 2.0. If a copy of the MPL was not distributed 177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#ifndef EIGEN_LEVENBERGMARQUARDT_H 207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#define EIGEN_LEVENBERGMARQUARDT_H 217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeznamespace Eigen { 247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeznamespace LevenbergMarquardtSpace { 257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez enum Status { 267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez NotStarted = -2, 277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Running = -1, 287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ImproperInputParameters = 0, 297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RelativeReductionTooSmall = 1, 307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RelativeErrorTooSmall = 2, 317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RelativeErrorAndReductionTooSmall = 3, 327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez CosinusTooSmall = 4, 337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez TooManyFunctionEvaluation = 5, 347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FtolTooSmall = 6, 357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez XtolTooSmall = 7, 367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez GtolTooSmall = 8, 377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez UserAsked = 9 387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez }; 397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate <typename _Scalar, int NX=Dynamic, int NY=Dynamic> 427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezstruct DenseFunctor 437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef _Scalar Scalar; 457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez enum { 467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez InputsAtCompileTime = NX, 477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ValuesAtCompileTime = NY 487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez }; 497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; 507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; 517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef ColPivHouseholderQR<JacobianType> QRSolver; 537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const int m_inputs, m_values; 547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int inputs() const { return m_inputs; } 597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int values() const { return m_values; } 607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //int operator()(const InputType &x, ValueType& fvec) { } 627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // should be defined in derived classes 637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //int df(const InputType &x, JacobianType& fjac) { } 657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // should be defined in derived classes 667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez}; 677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate <typename _Scalar, typename _Index> 697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezstruct SparseFunctor 707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef _Scalar Scalar; 727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef _Index Index; 737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,Dynamic,1> InputType; 747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,Dynamic,1> ValueType; 757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef SparseMatrix<Scalar, ColMajor, Index> JacobianType; 767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef SparseQR<JacobianType, COLAMDOrdering<int> > QRSolver; 777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez enum { 787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez InputsAtCompileTime = Dynamic, 797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ValuesAtCompileTime = Dynamic 807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez }; 817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int inputs() const { return m_inputs; } 857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez int values() const { return m_values; } 867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const int m_inputs, m_values; 887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //int operator()(const InputType &x, ValueType& fvec) { } 897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // to be defined in the functor 907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //int df(const InputType &x, JacobianType& fjac) { } 927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // to be defined in the functor if no automatic differentiation 937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez}; 957faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeznamespace internal { 967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate <typename QRSolver, typename VectorType> 977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezvoid lmpar2(const QRSolver &qr, const VectorType &diag, const VectorType &qtb, 987faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typename VectorType::Scalar m_delta, typename VectorType::Scalar &par, 997faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez VectorType &x); 1007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1017faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez/** 1027faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \ingroup NonLinearOptimization_Module 1037faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \brief Performs non linear optimization over a non-linear function, 1047faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * using a variant of the Levenberg Marquardt algorithm. 1057faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * 1067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * Check wikipedia for more information. 1077faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm 1087faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez */ 1097faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename _FunctorType> 1107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandezclass LevenbergMarquardt : internal::no_assignment_operator 1117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 1127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez public: 1137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef _FunctorType FunctorType; 1147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename FunctorType::QRSolver QRSolver; 1157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename FunctorType::JacobianType JacobianType; 1167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename JacobianType::Scalar Scalar; 1177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename JacobianType::RealScalar RealScalar; 1187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename JacobianType::Index Index; 1197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef typename QRSolver::Index PermIndex; 1207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef Matrix<Scalar,Dynamic,1> FVectorType; 1217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez typedef PermutationMatrix<Dynamic,Dynamic> PermutationType; 1227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez public: 1237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardt(FunctorType& functor) 1247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0), 1257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_isInitialized(false),m_info(InvalidInput) 1267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez resetParameters(); 1287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_useExternalScaling=false; 1297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status minimize(FVectorType &x); 1327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x); 1337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x); 1347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status lmder1( 1357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FVectorType &x, 1367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 1377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ); 1387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez static LevenbergMarquardtSpace::Status lmdif1( 1397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FunctorType &functor, 1407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FVectorType &x, 1417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index *nfev, 1427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 1437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ); 1447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** Sets the default parameters */ 1467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void resetParameters() 1477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 1487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_factor = 100.; 1497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_maxfev = 400; 1507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_ftol = std::sqrt(NumTraits<RealScalar>::epsilon()); 1517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_xtol = std::sqrt(NumTraits<RealScalar>::epsilon()); 1527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_gtol = 0. ; 1537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_epsfcn = 0. ; 1547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 1557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** Sets the tolerance for the norm of the solution vector*/ 1577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void setXtol(RealScalar xtol) { m_xtol = xtol; } 1587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** Sets the tolerance for the norm of the vector function*/ 1607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void setFtol(RealScalar ftol) { m_ftol = ftol; } 1617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** Sets the tolerance for the norm of the gradient of the error vector*/ 1637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void setGtol(RealScalar gtol) { m_gtol = gtol; } 1647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** Sets the step bound for the diagonal shift */ 1667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void setFactor(RealScalar factor) { m_factor = factor; } 1677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** Sets the error precision */ 1697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; } 1707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** Sets the maximum number of function evaluation */ 1727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void setMaxfev(Index maxfev) {m_maxfev = maxfev; } 1737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** Use an external Scaling. If set to true, pass a nonzero diagonal to diag() */ 1757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez void setExternalScaling(bool value) {m_useExternalScaling = value; } 1767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns a reference to the diagonal of the jacobian */ 1787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FVectorType& diag() {return m_diag; } 1797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns the number of iterations performed */ 1817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index iterations() { return m_iter; } 1827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns the number of functions evaluation */ 1847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index nfev() { return m_nfev; } 1857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns the number of jacobian evaluation */ 1877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index njev() { return m_njev; } 1887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns the norm of current vector function */ 1907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar fnorm() {return m_fnorm; } 1917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns the norm of the gradient of the error */ 1937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar gnorm() {return m_gnorm; } 1947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1957faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns the LevenbergMarquardt parameter */ 1967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar lm_param(void) { return m_par; } 1977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 1987faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns a reference to the current vector function 1997faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez */ 2007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FVectorType& fvec() {return m_fvec; } 2017faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2027faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns a reference to the matrix where the current Jacobian matrix is stored 2037faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez */ 2047faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez JacobianType& jacobian() {return m_fjac; } 2057faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** \returns a reference to the triangular matrix R from the QR of the jacobian matrix. 2077faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \sa jacobian() 2087faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez */ 2097faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez JacobianType& matrixR() {return m_rfactor; } 2107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** the permutation used in the QR factorization 2127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez */ 2137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez PermutationType permutation() {return m_permutation; } 2147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /** 2167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \brief Reports whether the minimization was successful 2177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \returns \c Success if the minimization was succesful, 2187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \c NumericalIssue if a numerical problem arises during the 2197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * minimization process, for exemple during the QR factorization 2207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \c NoConvergence if the minimization did not converge after 2217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * the maximum number of function evaluation allowed 2227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * \c InvalidInput if the input matrix is invalid 2237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez */ 2247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ComputationInfo info() const 2257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 2267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return m_info; 2287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 2297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez private: 2307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez JacobianType m_fjac; 2317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac 2327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FunctorType &m_functor; 2337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FVectorType m_fvec, m_qtf, m_diag; 2347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index n; 2357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index m; 2367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index m_nfev; 2377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index m_njev; 2387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar m_fnorm; // Norm of the current vector function 2397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar m_gnorm; //Norm of the gradient of the error 2407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar m_factor; // 2417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index m_maxfev; // Maximum number of function evaluation 2427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar m_ftol; //Tolerance in the norm of the vector function 2437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar m_xtol; // 2447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar m_gtol; //tolerance of the norm of the error gradient 2457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar m_epsfcn; // 2467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index m_iter; // Number of iterations performed 2477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar m_delta; 2487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez bool m_useExternalScaling; 2497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez PermutationType m_permutation; 2507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors 2517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez RealScalar m_par; 2527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez bool m_isInitialized; // Check whether the minimization step has been called 2537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ComputationInfo m_info; 2547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez}; 2557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename FunctorType> 2577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos HernandezLevenbergMarquardtSpace::Status 2587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos HernandezLevenbergMarquardt<FunctorType>::minimize(FVectorType &x) 2597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 2607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status status = minimizeInit(x); 2617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (status==LevenbergMarquardtSpace::ImproperInputParameters) { 2627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_isInitialized = true; 2637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return status; 2647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 2657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez do { 2667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// std::cout << " uv " << x.transpose() << "\n"; 2677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez status = minimizeOneStep(x); 2687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } while (status==LevenbergMarquardtSpace::Running); 2697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_isInitialized = true; 2707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return status; 2717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 2727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename FunctorType> 2747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos HernandezLevenbergMarquardtSpace::Status 2757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos HernandezLevenbergMarquardt<FunctorType>::minimizeInit(FVectorType &x) 2767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 2777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez n = x.size(); 2787faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m = m_functor.values(); 2797faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2807faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n); 2817faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_wa4.resize(m); 2827faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_fvec.resize(m); 2837faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez //FIXME Sparse Case : Allocate space for the jacobian 2847faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_fjac.resize(m, n); 2857faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez// m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative 2867faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (!m_useExternalScaling) 2877faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_diag.resize(n); 2887faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez eigen_assert( (!m_useExternalScaling || m_diag.size()==n) || "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'"); 2897faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_qtf.resize(n); 2907faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2917faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* Function Body */ 2927faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_nfev = 0; 2937faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_njev = 0; 2947faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 2957faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* check the input parameters for errors. */ 2967faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){ 2977faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_info = InvalidInput; 2987faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return LevenbergMarquardtSpace::ImproperInputParameters; 2997faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 3007faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3017faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (m_useExternalScaling) 3027faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez for (Index j = 0; j < n; ++j) 3037faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (m_diag[j] <= 0.) 3047faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez { 3057faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_info = InvalidInput; 3067faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return LevenbergMarquardtSpace::ImproperInputParameters; 3077faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez } 3087faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3097faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* evaluate the function at the starting point */ 3107faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* and calculate its norm. */ 3117faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_nfev = 1; 3127faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if ( m_functor(x, m_fvec) < 0) 3137faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return LevenbergMarquardtSpace::UserAsked; 3147faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_fnorm = m_fvec.stableNorm(); 3157faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3167faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* initialize levenberg-marquardt parameter and iteration counter. */ 3177faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_par = 0.; 3187faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_iter = 1; 3197faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3207faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return LevenbergMarquardtSpace::NotStarted; 3217faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 3227faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3237faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename FunctorType> 3247faaa9f3f0df9d23790277834d426c3d992ac3baCarlos HernandezLevenbergMarquardtSpace::Status 3257faaa9f3f0df9d23790277834d426c3d992ac3baCarlos HernandezLevenbergMarquardt<FunctorType>::lmder1( 3267faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FVectorType &x, 3277faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar tol 3287faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ) 3297faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 3307faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez n = x.size(); 3317faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m = m_functor.values(); 3327faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3337faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* check the input parameters for errors. */ 3347faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (n <= 0 || m < n || tol < 0.) 3357faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return LevenbergMarquardtSpace::ImproperInputParameters; 3367faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3377faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez resetParameters(); 3387faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_ftol = tol; 3397faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_xtol = tol; 3407faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez m_maxfev = 100*(n+1); 3417faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3427faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return minimize(x); 3437faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 3447faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3457faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3467faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandeztemplate<typename FunctorType> 3477faaa9f3f0df9d23790277834d426c3d992ac3baCarlos HernandezLevenbergMarquardtSpace::Status 3487faaa9f3f0df9d23790277834d426c3d992ac3baCarlos HernandezLevenbergMarquardt<FunctorType>::lmdif1( 3497faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FunctorType &functor, 3507faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez FVectorType &x, 3517faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index *nfev, 3527faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez const Scalar tol 3537faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez ) 3547faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez{ 3557faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index n = x.size(); 3567faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez Index m = functor.values(); 3577faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3587faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez /* check the input parameters for errors. */ 3597faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (n <= 0 || m < n || tol < 0.) 3607faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return LevenbergMarquardtSpace::ImproperInputParameters; 3617faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3627faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez NumericalDiff<FunctorType> numDiff(functor); 3637faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez // embedded LevenbergMarquardt 3647faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff); 3657faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez lm.setFtol(tol); 3667faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez lm.setXtol(tol); 3677faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez lm.setMaxfev(200*(n+1)); 3687faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3697faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x)); 3707faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez if (nfev) 3717faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez * nfev = lm.nfev(); 3727faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez return info; 3737faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} 3747faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3757faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez} // end namespace Eigen 3767faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez 3777faaa9f3f0df9d23790277834d426c3d992ac3baCarlos Hernandez#endif // EIGEN_LEVENBERGMARQUARDT_H 378