1 // This file is part of Eigen, a lightweight C++ template library 2 // for linear algebra. 3 // 4 // Copyright (C) 2009 Thomas Capricelli <orzel (at) freehackers.org> 5 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam (at) inria.fr> 6 // 7 // The algorithm of this class initially comes from MINPACK whose original authors are: 8 // Copyright Jorge More - Argonne National Laboratory 9 // Copyright Burt Garbow - Argonne National Laboratory 10 // Copyright Ken Hillstrom - Argonne National Laboratory 11 // 12 // This Source Code Form is subject to the terms of the Minpack license 13 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 14 // 15 // This Source Code Form is subject to the terms of the Mozilla 16 // Public License v. 2.0. If a copy of the MPL was not distributed 17 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 18 19 #ifndef EIGEN_LEVENBERGMARQUARDT_H 20 #define EIGEN_LEVENBERGMARQUARDT_H 21 22 23 namespace Eigen { 24 namespace LevenbergMarquardtSpace { 25 enum Status { 26 NotStarted = -2, 27 Running = -1, 28 ImproperInputParameters = 0, 29 RelativeReductionTooSmall = 1, 30 RelativeErrorTooSmall = 2, 31 RelativeErrorAndReductionTooSmall = 3, 32 CosinusTooSmall = 4, 33 TooManyFunctionEvaluation = 5, 34 FtolTooSmall = 6, 35 XtolTooSmall = 7, 36 GtolTooSmall = 8, 37 UserAsked = 9 38 }; 39 } 40 41 template <typename _Scalar, int NX=Dynamic, int NY=Dynamic> 42 struct DenseFunctor 43 { 44 typedef _Scalar Scalar; 45 enum { 46 InputsAtCompileTime = NX, 47 ValuesAtCompileTime = NY 48 }; 49 typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; 50 typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; 51 typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 52 typedef ColPivHouseholderQR<JacobianType> QRSolver; 53 const int m_inputs, m_values; 54 55 DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 56 DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 57 58 int inputs() const { return m_inputs; } 59 int values() const { return m_values; } 60 61 //int operator()(const InputType &x, ValueType& fvec) { } 62 // should be defined in derived classes 63 64 //int df(const InputType &x, JacobianType& fjac) { } 65 // should be defined in derived classes 66 }; 67 68 template <typename _Scalar, typename _Index> 69 struct SparseFunctor 70 { 71 typedef _Scalar Scalar; 72 typedef _Index Index; 73 typedef Matrix<Scalar,Dynamic,1> InputType; 74 typedef Matrix<Scalar,Dynamic,1> ValueType; 75 typedef SparseMatrix<Scalar, ColMajor, Index> JacobianType; 76 typedef SparseQR<JacobianType, COLAMDOrdering<int> > QRSolver; 77 enum { 78 InputsAtCompileTime = Dynamic, 79 ValuesAtCompileTime = Dynamic 80 }; 81 82 SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 83 84 int inputs() const { return m_inputs; } 85 int values() const { return m_values; } 86 87 const int m_inputs, m_values; 88 //int operator()(const InputType &x, ValueType& fvec) { } 89 // to be defined in the functor 90 91 //int df(const InputType &x, JacobianType& fjac) { } 92 // to be defined in the functor if no automatic differentiation 93 94 }; 95 namespace internal { 96 template <typename QRSolver, typename VectorType> 97 void lmpar2(const QRSolver &qr, const VectorType &diag, const VectorType &qtb, 98 typename VectorType::Scalar m_delta, typename VectorType::Scalar &par, 99 VectorType &x); 100 } 101 /** 102 * \ingroup NonLinearOptimization_Module 103 * \brief Performs non linear optimization over a non-linear function, 104 * using a variant of the Levenberg Marquardt algorithm. 105 * 106 * Check wikipedia for more information. 107 * http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm 108 */ 109 template<typename _FunctorType> 110 class LevenbergMarquardt : internal::no_assignment_operator 111 { 112 public: 113 typedef _FunctorType FunctorType; 114 typedef typename FunctorType::QRSolver QRSolver; 115 typedef typename FunctorType::JacobianType JacobianType; 116 typedef typename JacobianType::Scalar Scalar; 117 typedef typename JacobianType::RealScalar RealScalar; 118 typedef typename QRSolver::StorageIndex PermIndex; 119 typedef Matrix<Scalar,Dynamic,1> FVectorType; 120 typedef PermutationMatrix<Dynamic,Dynamic> PermutationType; 121 public: 122 LevenbergMarquardt(FunctorType& functor) 123 : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0), 124 m_isInitialized(false),m_info(InvalidInput) 125 { 126 resetParameters(); 127 m_useExternalScaling=false; 128 } 129 130 LevenbergMarquardtSpace::Status minimize(FVectorType &x); 131 LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x); 132 LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x); 133 LevenbergMarquardtSpace::Status lmder1( 134 FVectorType &x, 135 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 136 ); 137 static LevenbergMarquardtSpace::Status lmdif1( 138 FunctorType &functor, 139 FVectorType &x, 140 Index *nfev, 141 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 142 ); 143 144 /** Sets the default parameters */ 145 void resetParameters() 146 { 147 using std::sqrt; 148 149 m_factor = 100.; 150 m_maxfev = 400; 151 m_ftol = sqrt(NumTraits<RealScalar>::epsilon()); 152 m_xtol = sqrt(NumTraits<RealScalar>::epsilon()); 153 m_gtol = 0. ; 154 m_epsfcn = 0. ; 155 } 156 157 /** Sets the tolerance for the norm of the solution vector*/ 158 void setXtol(RealScalar xtol) { m_xtol = xtol; } 159 160 /** Sets the tolerance for the norm of the vector function*/ 161 void setFtol(RealScalar ftol) { m_ftol = ftol; } 162 163 /** Sets the tolerance for the norm of the gradient of the error vector*/ 164 void setGtol(RealScalar gtol) { m_gtol = gtol; } 165 166 /** Sets the step bound for the diagonal shift */ 167 void setFactor(RealScalar factor) { m_factor = factor; } 168 169 /** Sets the error precision */ 170 void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; } 171 172 /** Sets the maximum number of function evaluation */ 173 void setMaxfev(Index maxfev) {m_maxfev = maxfev; } 174 175 /** Use an external Scaling. If set to true, pass a nonzero diagonal to diag() */ 176 void setExternalScaling(bool value) {m_useExternalScaling = value; } 177 178 /** \returns the tolerance for the norm of the solution vector */ 179 RealScalar xtol() const {return m_xtol; } 180 181 /** \returns the tolerance for the norm of the vector function */ 182 RealScalar ftol() const {return m_ftol; } 183 184 /** \returns the tolerance for the norm of the gradient of the error vector */ 185 RealScalar gtol() const {return m_gtol; } 186 187 /** \returns the step bound for the diagonal shift */ 188 RealScalar factor() const {return m_factor; } 189 190 /** \returns the error precision */ 191 RealScalar epsilon() const {return m_epsfcn; } 192 193 /** \returns the maximum number of function evaluation */ 194 Index maxfev() const {return m_maxfev; } 195 196 /** \returns a reference to the diagonal of the jacobian */ 197 FVectorType& diag() {return m_diag; } 198 199 /** \returns the number of iterations performed */ 200 Index iterations() { return m_iter; } 201 202 /** \returns the number of functions evaluation */ 203 Index nfev() { return m_nfev; } 204 205 /** \returns the number of jacobian evaluation */ 206 Index njev() { return m_njev; } 207 208 /** \returns the norm of current vector function */ 209 RealScalar fnorm() {return m_fnorm; } 210 211 /** \returns the norm of the gradient of the error */ 212 RealScalar gnorm() {return m_gnorm; } 213 214 /** \returns the LevenbergMarquardt parameter */ 215 RealScalar lm_param(void) { return m_par; } 216 217 /** \returns a reference to the current vector function 218 */ 219 FVectorType& fvec() {return m_fvec; } 220 221 /** \returns a reference to the matrix where the current Jacobian matrix is stored 222 */ 223 JacobianType& jacobian() {return m_fjac; } 224 225 /** \returns a reference to the triangular matrix R from the QR of the jacobian matrix. 226 * \sa jacobian() 227 */ 228 JacobianType& matrixR() {return m_rfactor; } 229 230 /** the permutation used in the QR factorization 231 */ 232 PermutationType permutation() {return m_permutation; } 233 234 /** 235 * \brief Reports whether the minimization was successful 236 * \returns \c Success if the minimization was succesful, 237 * \c NumericalIssue if a numerical problem arises during the 238 * minimization process, for exemple during the QR factorization 239 * \c NoConvergence if the minimization did not converge after 240 * the maximum number of function evaluation allowed 241 * \c InvalidInput if the input matrix is invalid 242 */ 243 ComputationInfo info() const 244 { 245 246 return m_info; 247 } 248 private: 249 JacobianType m_fjac; 250 JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac 251 FunctorType &m_functor; 252 FVectorType m_fvec, m_qtf, m_diag; 253 Index n; 254 Index m; 255 Index m_nfev; 256 Index m_njev; 257 RealScalar m_fnorm; // Norm of the current vector function 258 RealScalar m_gnorm; //Norm of the gradient of the error 259 RealScalar m_factor; // 260 Index m_maxfev; // Maximum number of function evaluation 261 RealScalar m_ftol; //Tolerance in the norm of the vector function 262 RealScalar m_xtol; // 263 RealScalar m_gtol; //tolerance of the norm of the error gradient 264 RealScalar m_epsfcn; // 265 Index m_iter; // Number of iterations performed 266 RealScalar m_delta; 267 bool m_useExternalScaling; 268 PermutationType m_permutation; 269 FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors 270 RealScalar m_par; 271 bool m_isInitialized; // Check whether the minimization step has been called 272 ComputationInfo m_info; 273 }; 274 275 template<typename FunctorType> 276 LevenbergMarquardtSpace::Status 277 LevenbergMarquardt<FunctorType>::minimize(FVectorType &x) 278 { 279 LevenbergMarquardtSpace::Status status = minimizeInit(x); 280 if (status==LevenbergMarquardtSpace::ImproperInputParameters) { 281 m_isInitialized = true; 282 return status; 283 } 284 do { 285 // std::cout << " uv " << x.transpose() << "\n"; 286 status = minimizeOneStep(x); 287 } while (status==LevenbergMarquardtSpace::Running); 288 m_isInitialized = true; 289 return status; 290 } 291 292 template<typename FunctorType> 293 LevenbergMarquardtSpace::Status 294 LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType &x) 295 { 296 n = x.size(); 297 m = m_functor.values(); 298 299 m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n); 300 m_wa4.resize(m); 301 m_fvec.resize(m); 302 //FIXME Sparse Case : Allocate space for the jacobian 303 m_fjac.resize(m, n); 304 // m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative 305 if (!m_useExternalScaling) 306 m_diag.resize(n); 307 eigen_assert( (!m_useExternalScaling || m_diag.size()==n) && "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'"); 308 m_qtf.resize(n); 309 310 /* Function Body */ 311 m_nfev = 0; 312 m_njev = 0; 313 314 /* check the input parameters for errors. */ 315 if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){ 316 m_info = InvalidInput; 317 return LevenbergMarquardtSpace::ImproperInputParameters; 318 } 319 320 if (m_useExternalScaling) 321 for (Index j = 0; j < n; ++j) 322 if (m_diag[j] <= 0.) 323 { 324 m_info = InvalidInput; 325 return LevenbergMarquardtSpace::ImproperInputParameters; 326 } 327 328 /* evaluate the function at the starting point */ 329 /* and calculate its norm. */ 330 m_nfev = 1; 331 if ( m_functor(x, m_fvec) < 0) 332 return LevenbergMarquardtSpace::UserAsked; 333 m_fnorm = m_fvec.stableNorm(); 334 335 /* initialize levenberg-marquardt parameter and iteration counter. */ 336 m_par = 0.; 337 m_iter = 1; 338 339 return LevenbergMarquardtSpace::NotStarted; 340 } 341 342 template<typename FunctorType> 343 LevenbergMarquardtSpace::Status 344 LevenbergMarquardt<FunctorType>::lmder1( 345 FVectorType &x, 346 const Scalar tol 347 ) 348 { 349 n = x.size(); 350 m = m_functor.values(); 351 352 /* check the input parameters for errors. */ 353 if (n <= 0 || m < n || tol < 0.) 354 return LevenbergMarquardtSpace::ImproperInputParameters; 355 356 resetParameters(); 357 m_ftol = tol; 358 m_xtol = tol; 359 m_maxfev = 100*(n+1); 360 361 return minimize(x); 362 } 363 364 365 template<typename FunctorType> 366 LevenbergMarquardtSpace::Status 367 LevenbergMarquardt<FunctorType>::lmdif1( 368 FunctorType &functor, 369 FVectorType &x, 370 Index *nfev, 371 const Scalar tol 372 ) 373 { 374 Index n = x.size(); 375 Index m = functor.values(); 376 377 /* check the input parameters for errors. */ 378 if (n <= 0 || m < n || tol < 0.) 379 return LevenbergMarquardtSpace::ImproperInputParameters; 380 381 NumericalDiff<FunctorType> numDiff(functor); 382 // embedded LevenbergMarquardt 383 LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff); 384 lm.setFtol(tol); 385 lm.setXtol(tol); 386 lm.setMaxfev(200*(n+1)); 387 388 LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x)); 389 if (nfev) 390 * nfev = lm.nfev(); 391 return info; 392 } 393 394 } // end namespace Eigen 395 396 #endif // EIGEN_LEVENBERGMARQUARDT_H 397