![]() |
Eigen-unsupported
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> 00005 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr> 00006 // 00007 // The algorithm of this class initially comes from MINPACK whose original authors are: 00008 // Copyright Jorge More - Argonne National Laboratory 00009 // Copyright Burt Garbow - Argonne National Laboratory 00010 // Copyright Ken Hillstrom - Argonne National Laboratory 00011 // 00012 // This Source Code Form is subject to the terms of the Minpack license 00013 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 00014 // 00015 // This Source Code Form is subject to the terms of the Mozilla 00016 // Public License v. 2.0. If a copy of the MPL was not distributed 00017 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. 00018 00019 #ifndef EIGEN_LEVENBERGMARQUARDT_H 00020 #define EIGEN_LEVENBERGMARQUARDT_H 00021 00022 00023 namespace Eigen { 00024 namespace LevenbergMarquardtSpace { 00025 enum Status { 00026 NotStarted = -2, 00027 Running = -1, 00028 ImproperInputParameters = 0, 00029 RelativeReductionTooSmall = 1, 00030 RelativeErrorTooSmall = 2, 00031 RelativeErrorAndReductionTooSmall = 3, 00032 CosinusTooSmall = 4, 00033 TooManyFunctionEvaluation = 5, 00034 FtolTooSmall = 6, 00035 XtolTooSmall = 7, 00036 GtolTooSmall = 8, 00037 UserAsked = 9 00038 }; 00039 } 00040 00041 template <typename _Scalar, int NX=Dynamic, int NY=Dynamic> 00042 struct DenseFunctor 00043 { 00044 typedef _Scalar Scalar; 00045 enum { 00046 InputsAtCompileTime = NX, 00047 ValuesAtCompileTime = NY 00048 }; 00049 typedef Matrix<Scalar,InputsAtCompileTime,1> InputType; 00050 typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType; 00051 typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType; 00052 typedef ColPivHouseholderQR<JacobianType> QRSolver; 00053 const int m_inputs, m_values; 00054 00055 DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {} 00056 DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 00057 00058 int inputs() const { return m_inputs; } 00059 int values() const { return m_values; } 00060 00061 //int operator()(const InputType &x, ValueType& fvec) { } 00062 // should be defined in derived classes 00063 00064 //int df(const InputType &x, JacobianType& fjac) { } 00065 // should be defined in derived classes 00066 }; 00067 00068 template <typename _Scalar, typename _Index> 00069 struct SparseFunctor 00070 { 00071 typedef _Scalar Scalar; 00072 typedef _Index Index; 00073 typedef Matrix<Scalar,Dynamic,1> InputType; 00074 typedef Matrix<Scalar,Dynamic,1> ValueType; 00075 typedef SparseMatrix<Scalar, ColMajor, Index> JacobianType; 00076 typedef SparseQR<JacobianType, COLAMDOrdering<int> > QRSolver; 00077 enum { 00078 InputsAtCompileTime = Dynamic, 00079 ValuesAtCompileTime = Dynamic 00080 }; 00081 00082 SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {} 00083 00084 int inputs() const { return m_inputs; } 00085 int values() const { return m_values; } 00086 00087 const int m_inputs, m_values; 00088 //int operator()(const InputType &x, ValueType& fvec) { } 00089 // to be defined in the functor 00090 00091 //int df(const InputType &x, JacobianType& fjac) { } 00092 // to be defined in the functor if no automatic differentiation 00093 00094 }; 00095 namespace internal { 00096 template <typename QRSolver, typename VectorType> 00097 void lmpar2(const QRSolver &qr, const VectorType &diag, const VectorType &qtb, 00098 typename VectorType::Scalar m_delta, typename VectorType::Scalar &par, 00099 VectorType &x); 00100 } 00109 template<typename _FunctorType> 00110 class LevenbergMarquardt : internal::no_assignment_operator 00111 { 00112 public: 00113 typedef _FunctorType FunctorType; 00114 typedef typename FunctorType::QRSolver QRSolver; 00115 typedef typename FunctorType::JacobianType JacobianType; 00116 typedef typename JacobianType::Scalar Scalar; 00117 typedef typename JacobianType::RealScalar RealScalar; 00118 typedef typename QRSolver::StorageIndex PermIndex; 00119 typedef Matrix<Scalar,Dynamic,1> FVectorType; 00120 typedef PermutationMatrix<Dynamic,Dynamic> PermutationType; 00121 public: 00122 LevenbergMarquardt(FunctorType& functor) 00123 : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0), 00124 m_isInitialized(false),m_info(InvalidInput) 00125 { 00126 resetParameters(); 00127 m_useExternalScaling=false; 00128 } 00129 00130 LevenbergMarquardtSpace::Status minimize(FVectorType &x); 00131 LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x); 00132 LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x); 00133 LevenbergMarquardtSpace::Status lmder1( 00134 FVectorType &x, 00135 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 00136 ); 00137 static LevenbergMarquardtSpace::Status lmdif1( 00138 FunctorType &functor, 00139 FVectorType &x, 00140 Index *nfev, 00141 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon()) 00142 ); 00143 00145 void resetParameters() 00146 { 00147 using std::sqrt; 00148 00149 m_factor = 100.; 00150 m_maxfev = 400; 00151 m_ftol = sqrt(NumTraits<RealScalar>::epsilon()); 00152 m_xtol = sqrt(NumTraits<RealScalar>::epsilon()); 00153 m_gtol = 0. ; 00154 m_epsfcn = 0. ; 00155 } 00156 00158 void setXtol(RealScalar xtol) { m_xtol = xtol; } 00159 00161 void setFtol(RealScalar ftol) { m_ftol = ftol; } 00162 00164 void setGtol(RealScalar gtol) { m_gtol = gtol; } 00165 00167 void setFactor(RealScalar factor) { m_factor = factor; } 00168 00170 void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; } 00171 00173 void setMaxfev(Index maxfev) {m_maxfev = maxfev; } 00174 00176 void setExternalScaling(bool value) {m_useExternalScaling = value; } 00177 00179 RealScalar xtol() const {return m_xtol; } 00180 00182 RealScalar ftol() const {return m_ftol; } 00183 00185 RealScalar gtol() const {return m_gtol; } 00186 00188 RealScalar factor() const {return m_factor; } 00189 00191 RealScalar epsilon() const {return m_epsfcn; } 00192 00194 Index maxfev() const {return m_maxfev; } 00195 00197 FVectorType& diag() {return m_diag; } 00198 00200 Index iterations() { return m_iter; } 00201 00203 Index nfev() { return m_nfev; } 00204 00206 Index njev() { return m_njev; } 00207 00209 RealScalar fnorm() {return m_fnorm; } 00210 00212 RealScalar gnorm() {return m_gnorm; } 00213 00215 RealScalar lm_param(void) { return m_par; } 00216 00219 FVectorType& fvec() {return m_fvec; } 00220 00223 JacobianType& jacobian() {return m_fjac; } 00224 00228 JacobianType& matrixR() {return m_rfactor; } 00229 00232 PermutationType permutation() {return m_permutation; } 00233 00243 ComputationInfo info() const 00244 { 00245 00246 return m_info; 00247 } 00248 private: 00249 JacobianType m_fjac; 00250 JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac 00251 FunctorType &m_functor; 00252 FVectorType m_fvec, m_qtf, m_diag; 00253 Index n; 00254 Index m; 00255 Index m_nfev; 00256 Index m_njev; 00257 RealScalar m_fnorm; // Norm of the current vector function 00258 RealScalar m_gnorm; //Norm of the gradient of the error 00259 RealScalar m_factor; // 00260 Index m_maxfev; // Maximum number of function evaluation 00261 RealScalar m_ftol; //Tolerance in the norm of the vector function 00262 RealScalar m_xtol; // 00263 RealScalar m_gtol; //tolerance of the norm of the error gradient 00264 RealScalar m_epsfcn; // 00265 Index m_iter; // Number of iterations performed 00266 RealScalar m_delta; 00267 bool m_useExternalScaling; 00268 PermutationType m_permutation; 00269 FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors 00270 RealScalar m_par; 00271 bool m_isInitialized; // Check whether the minimization step has been called 00272 ComputationInfo m_info; 00273 }; 00274 00275 template<typename FunctorType> 00276 LevenbergMarquardtSpace::Status 00277 LevenbergMarquardt<FunctorType>::minimize(FVectorType &x) 00278 { 00279 LevenbergMarquardtSpace::Status status = minimizeInit(x); 00280 if (status==LevenbergMarquardtSpace::ImproperInputParameters) { 00281 m_isInitialized = true; 00282 return status; 00283 } 00284 do { 00285 // std::cout << " uv " << x.transpose() << "\n"; 00286 status = minimizeOneStep(x); 00287 } while (status==LevenbergMarquardtSpace::Running); 00288 m_isInitialized = true; 00289 return status; 00290 } 00291 00292 template<typename FunctorType> 00293 LevenbergMarquardtSpace::Status 00294 LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType &x) 00295 { 00296 n = x.size(); 00297 m = m_functor.values(); 00298 00299 m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n); 00300 m_wa4.resize(m); 00301 m_fvec.resize(m); 00302 //FIXME Sparse Case : Allocate space for the jacobian 00303 m_fjac.resize(m, n); 00304 // m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative 00305 if (!m_useExternalScaling) 00306 m_diag.resize(n); 00307 eigen_assert( (!m_useExternalScaling || m_diag.size()==n) && "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'"); 00308 m_qtf.resize(n); 00309 00310 /* Function Body */ 00311 m_nfev = 0; 00312 m_njev = 0; 00313 00314 /* check the input parameters for errors. */ 00315 if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){ 00316 m_info = InvalidInput; 00317 return LevenbergMarquardtSpace::ImproperInputParameters; 00318 } 00319 00320 if (m_useExternalScaling) 00321 for (Index j = 0; j < n; ++j) 00322 if (m_diag[j] <= 0.) 00323 { 00324 m_info = InvalidInput; 00325 return LevenbergMarquardtSpace::ImproperInputParameters; 00326 } 00327 00328 /* evaluate the function at the starting point */ 00329 /* and calculate its norm. */ 00330 m_nfev = 1; 00331 if ( m_functor(x, m_fvec) < 0) 00332 return LevenbergMarquardtSpace::UserAsked; 00333 m_fnorm = m_fvec.stableNorm(); 00334 00335 /* initialize levenberg-marquardt parameter and iteration counter. */ 00336 m_par = 0.; 00337 m_iter = 1; 00338 00339 return LevenbergMarquardtSpace::NotStarted; 00340 } 00341 00342 template<typename FunctorType> 00343 LevenbergMarquardtSpace::Status 00344 LevenbergMarquardt<FunctorType>::lmder1( 00345 FVectorType &x, 00346 const Scalar tol 00347 ) 00348 { 00349 n = x.size(); 00350 m = m_functor.values(); 00351 00352 /* check the input parameters for errors. */ 00353 if (n <= 0 || m < n || tol < 0.) 00354 return LevenbergMarquardtSpace::ImproperInputParameters; 00355 00356 resetParameters(); 00357 m_ftol = tol; 00358 m_xtol = tol; 00359 m_maxfev = 100*(n+1); 00360 00361 return minimize(x); 00362 } 00363 00364 00365 template<typename FunctorType> 00366 LevenbergMarquardtSpace::Status 00367 LevenbergMarquardt<FunctorType>::lmdif1( 00368 FunctorType &functor, 00369 FVectorType &x, 00370 Index *nfev, 00371 const Scalar tol 00372 ) 00373 { 00374 Index n = x.size(); 00375 Index m = functor.values(); 00376 00377 /* check the input parameters for errors. */ 00378 if (n <= 0 || m < n || tol < 0.) 00379 return LevenbergMarquardtSpace::ImproperInputParameters; 00380 00381 NumericalDiff<FunctorType> numDiff(functor); 00382 // embedded LevenbergMarquardt 00383 LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff); 00384 lm.setFtol(tol); 00385 lm.setXtol(tol); 00386 lm.setMaxfev(200*(n+1)); 00387 00388 LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x)); 00389 if (nfev) 00390 * nfev = lm.nfev(); 00391 return info; 00392 } 00393 00394 } // end namespace Eigen 00395 00396 #endif // EIGEN_LEVENBERGMARQUARDT_H