1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
5 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@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
DenseFunctorDenseFunctor55 DenseFunctor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
DenseFunctorDenseFunctor56 DenseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
57
inputsDenseFunctor58 int inputs() const { return m_inputs; }
valuesDenseFunctor59 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
SparseFunctorSparseFunctor82 SparseFunctor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
83
inputsSparseFunctor84 int inputs() const { return m_inputs; }
valuesSparseFunctor85 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 JacobianType::Index Index;
119 typedef typename QRSolver::Index PermIndex;
120 typedef Matrix<Scalar,Dynamic,1> FVectorType;
121 typedef PermutationMatrix<Dynamic,Dynamic> PermutationType;
122 public:
LevenbergMarquardt(FunctorType & functor)123 LevenbergMarquardt(FunctorType& functor)
124 : m_functor(functor),m_nfev(0),m_njev(0),m_fnorm(0.0),m_gnorm(0),
125 m_isInitialized(false),m_info(InvalidInput)
126 {
127 resetParameters();
128 m_useExternalScaling=false;
129 }
130
131 LevenbergMarquardtSpace::Status minimize(FVectorType &x);
132 LevenbergMarquardtSpace::Status minimizeInit(FVectorType &x);
133 LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x);
134 LevenbergMarquardtSpace::Status lmder1(
135 FVectorType &x,
136 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
137 );
138 static LevenbergMarquardtSpace::Status lmdif1(
139 FunctorType &functor,
140 FVectorType &x,
141 Index *nfev,
142 const Scalar tol = std::sqrt(NumTraits<Scalar>::epsilon())
143 );
144
145 /** Sets the default parameters */
resetParameters()146 void resetParameters()
147 {
148 m_factor = 100.;
149 m_maxfev = 400;
150 m_ftol = std::sqrt(NumTraits<RealScalar>::epsilon());
151 m_xtol = std::sqrt(NumTraits<RealScalar>::epsilon());
152 m_gtol = 0. ;
153 m_epsfcn = 0. ;
154 }
155
156 /** Sets the tolerance for the norm of the solution vector*/
setXtol(RealScalar xtol)157 void setXtol(RealScalar xtol) { m_xtol = xtol; }
158
159 /** Sets the tolerance for the norm of the vector function*/
setFtol(RealScalar ftol)160 void setFtol(RealScalar ftol) { m_ftol = ftol; }
161
162 /** Sets the tolerance for the norm of the gradient of the error vector*/
setGtol(RealScalar gtol)163 void setGtol(RealScalar gtol) { m_gtol = gtol; }
164
165 /** Sets the step bound for the diagonal shift */
setFactor(RealScalar factor)166 void setFactor(RealScalar factor) { m_factor = factor; }
167
168 /** Sets the error precision */
setEpsilon(RealScalar epsfcn)169 void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; }
170
171 /** Sets the maximum number of function evaluation */
setMaxfev(Index maxfev)172 void setMaxfev(Index maxfev) {m_maxfev = maxfev; }
173
174 /** Use an external Scaling. If set to true, pass a nonzero diagonal to diag() */
setExternalScaling(bool value)175 void setExternalScaling(bool value) {m_useExternalScaling = value; }
176
177 /** \returns a reference to the diagonal of the jacobian */
diag()178 FVectorType& diag() {return m_diag; }
179
180 /** \returns the number of iterations performed */
iterations()181 Index iterations() { return m_iter; }
182
183 /** \returns the number of functions evaluation */
nfev()184 Index nfev() { return m_nfev; }
185
186 /** \returns the number of jacobian evaluation */
njev()187 Index njev() { return m_njev; }
188
189 /** \returns the norm of current vector function */
fnorm()190 RealScalar fnorm() {return m_fnorm; }
191
192 /** \returns the norm of the gradient of the error */
gnorm()193 RealScalar gnorm() {return m_gnorm; }
194
195 /** \returns the LevenbergMarquardt parameter */
lm_param(void)196 RealScalar lm_param(void) { return m_par; }
197
198 /** \returns a reference to the current vector function
199 */
fvec()200 FVectorType& fvec() {return m_fvec; }
201
202 /** \returns a reference to the matrix where the current Jacobian matrix is stored
203 */
jacobian()204 JacobianType& jacobian() {return m_fjac; }
205
206 /** \returns a reference to the triangular matrix R from the QR of the jacobian matrix.
207 * \sa jacobian()
208 */
matrixR()209 JacobianType& matrixR() {return m_rfactor; }
210
211 /** the permutation used in the QR factorization
212 */
permutation()213 PermutationType permutation() {return m_permutation; }
214
215 /**
216 * \brief Reports whether the minimization was successful
217 * \returns \c Success if the minimization was succesful,
218 * \c NumericalIssue if a numerical problem arises during the
219 * minimization process, for exemple during the QR factorization
220 * \c NoConvergence if the minimization did not converge after
221 * the maximum number of function evaluation allowed
222 * \c InvalidInput if the input matrix is invalid
223 */
info()224 ComputationInfo info() const
225 {
226
227 return m_info;
228 }
229 private:
230 JacobianType m_fjac;
231 JacobianType m_rfactor; // The triangular matrix R from the QR of the jacobian matrix m_fjac
232 FunctorType &m_functor;
233 FVectorType m_fvec, m_qtf, m_diag;
234 Index n;
235 Index m;
236 Index m_nfev;
237 Index m_njev;
238 RealScalar m_fnorm; // Norm of the current vector function
239 RealScalar m_gnorm; //Norm of the gradient of the error
240 RealScalar m_factor; //
241 Index m_maxfev; // Maximum number of function evaluation
242 RealScalar m_ftol; //Tolerance in the norm of the vector function
243 RealScalar m_xtol; //
244 RealScalar m_gtol; //tolerance of the norm of the error gradient
245 RealScalar m_epsfcn; //
246 Index m_iter; // Number of iterations performed
247 RealScalar m_delta;
248 bool m_useExternalScaling;
249 PermutationType m_permutation;
250 FVectorType m_wa1, m_wa2, m_wa3, m_wa4; //Temporary vectors
251 RealScalar m_par;
252 bool m_isInitialized; // Check whether the minimization step has been called
253 ComputationInfo m_info;
254 };
255
256 template<typename FunctorType>
257 LevenbergMarquardtSpace::Status
minimize(FVectorType & x)258 LevenbergMarquardt<FunctorType>::minimize(FVectorType &x)
259 {
260 LevenbergMarquardtSpace::Status status = minimizeInit(x);
261 if (status==LevenbergMarquardtSpace::ImproperInputParameters) {
262 m_isInitialized = true;
263 return status;
264 }
265 do {
266 // std::cout << " uv " << x.transpose() << "\n";
267 status = minimizeOneStep(x);
268 } while (status==LevenbergMarquardtSpace::Running);
269 m_isInitialized = true;
270 return status;
271 }
272
273 template<typename FunctorType>
274 LevenbergMarquardtSpace::Status
minimizeInit(FVectorType & x)275 LevenbergMarquardt<FunctorType>::minimizeInit(FVectorType &x)
276 {
277 n = x.size();
278 m = m_functor.values();
279
280 m_wa1.resize(n); m_wa2.resize(n); m_wa3.resize(n);
281 m_wa4.resize(m);
282 m_fvec.resize(m);
283 //FIXME Sparse Case : Allocate space for the jacobian
284 m_fjac.resize(m, n);
285 // m_fjac.reserve(VectorXi::Constant(n,5)); // FIXME Find a better alternative
286 if (!m_useExternalScaling)
287 m_diag.resize(n);
288 eigen_assert( (!m_useExternalScaling || m_diag.size()==n) || "When m_useExternalScaling is set, the caller must provide a valid 'm_diag'");
289 m_qtf.resize(n);
290
291 /* Function Body */
292 m_nfev = 0;
293 m_njev = 0;
294
295 /* check the input parameters for errors. */
296 if (n <= 0 || m < n || m_ftol < 0. || m_xtol < 0. || m_gtol < 0. || m_maxfev <= 0 || m_factor <= 0.){
297 m_info = InvalidInput;
298 return LevenbergMarquardtSpace::ImproperInputParameters;
299 }
300
301 if (m_useExternalScaling)
302 for (Index j = 0; j < n; ++j)
303 if (m_diag[j] <= 0.)
304 {
305 m_info = InvalidInput;
306 return LevenbergMarquardtSpace::ImproperInputParameters;
307 }
308
309 /* evaluate the function at the starting point */
310 /* and calculate its norm. */
311 m_nfev = 1;
312 if ( m_functor(x, m_fvec) < 0)
313 return LevenbergMarquardtSpace::UserAsked;
314 m_fnorm = m_fvec.stableNorm();
315
316 /* initialize levenberg-marquardt parameter and iteration counter. */
317 m_par = 0.;
318 m_iter = 1;
319
320 return LevenbergMarquardtSpace::NotStarted;
321 }
322
323 template<typename FunctorType>
324 LevenbergMarquardtSpace::Status
lmder1(FVectorType & x,const Scalar tol)325 LevenbergMarquardt<FunctorType>::lmder1(
326 FVectorType &x,
327 const Scalar tol
328 )
329 {
330 n = x.size();
331 m = m_functor.values();
332
333 /* check the input parameters for errors. */
334 if (n <= 0 || m < n || tol < 0.)
335 return LevenbergMarquardtSpace::ImproperInputParameters;
336
337 resetParameters();
338 m_ftol = tol;
339 m_xtol = tol;
340 m_maxfev = 100*(n+1);
341
342 return minimize(x);
343 }
344
345
346 template<typename FunctorType>
347 LevenbergMarquardtSpace::Status
lmdif1(FunctorType & functor,FVectorType & x,Index * nfev,const Scalar tol)348 LevenbergMarquardt<FunctorType>::lmdif1(
349 FunctorType &functor,
350 FVectorType &x,
351 Index *nfev,
352 const Scalar tol
353 )
354 {
355 Index n = x.size();
356 Index m = functor.values();
357
358 /* check the input parameters for errors. */
359 if (n <= 0 || m < n || tol < 0.)
360 return LevenbergMarquardtSpace::ImproperInputParameters;
361
362 NumericalDiff<FunctorType> numDiff(functor);
363 // embedded LevenbergMarquardt
364 LevenbergMarquardt<NumericalDiff<FunctorType> > lm(numDiff);
365 lm.setFtol(tol);
366 lm.setXtol(tol);
367 lm.setMaxfev(200*(n+1));
368
369 LevenbergMarquardtSpace::Status info = LevenbergMarquardtSpace::Status(lm.minimize(x));
370 if (nfev)
371 * nfev = lm.nfev();
372 return info;
373 }
374
375 } // end namespace Eigen
376
377 #endif // EIGEN_LEVENBERGMARQUARDT_H
378