Please, help us to better know about our user community by answering the following short survey: https://forms.gle/wpyrxWi18ox9Z5ae9
 
Loading...
Searching...
No Matches
LevenbergMarquardt/LevenbergMarquardt.h
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
23namespace Eigen {
24namespace 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
41template <typename _Scalar, int NX=Dynamic, int NY=Dynamic>
42struct 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
68template <typename _Scalar, typename _Index>
69struct 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};
95namespace internal {
96template <typename QRSolver, typename VectorType>
97void lmpar2(const QRSolver &qr, const VectorType &diag, const VectorType &qtb,
98 typename VectorType::Scalar m_delta, typename VectorType::Scalar &par,
99 VectorType &x);
100 }
109template<typename _FunctorType>
110class 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;
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 {
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
146 {
147 using std::sqrt;
148
149 m_factor = 100.;
150 m_maxfev = 400;
153 m_gtol = 0. ;
154 m_epsfcn = 0. ;
155 }
156
158 void setXtol(RealScalar xtol) { m_xtol = xtol; }
159
161 void setFtol(RealScalar ftol) { m_ftol = ftol; }
162
164 void setGtol(RealScalar gtol) { m_gtol = gtol; }
165
167 void setFactor(RealScalar factor) { m_factor = factor; }
168
170 void setEpsilon (RealScalar epsfcn) { m_epsfcn = epsfcn; }
171
173 void setMaxfev(Index maxfev) {m_maxfev = maxfev; }
174
176 void setExternalScaling(bool value) {m_useExternalScaling = value; }
177
179 RealScalar xtol() const {return m_xtol; }
180
182 RealScalar ftol() const {return m_ftol; }
183
185 RealScalar gtol() const {return m_gtol; }
186
188 RealScalar factor() const {return m_factor; }
189
191 RealScalar epsilon() const {return m_epsfcn; }
192
194 Index maxfev() const {return m_maxfev; }
195
197 FVectorType& diag() {return m_diag; }
198
200 Index iterations() { return m_iter; }
201
203 Index nfev() { return m_nfev; }
204
206 Index njev() { return m_njev; }
207
209 RealScalar fnorm() {return m_fnorm; }
210
212 RealScalar gnorm() {return m_gnorm; }
213
215 RealScalar lm_param(void) { return m_par; }
216
219 FVectorType& fvec() {return m_fvec; }
220
223 JacobianType& jacobian() {return m_fjac; }
224
228 JacobianType& matrixR() {return m_rfactor; }
229
232 PermutationType permutation() {return m_permutation; }
233
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
275template<typename FunctorType>
276LevenbergMarquardtSpace::Status
277LevenbergMarquardt<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
292template<typename FunctorType>
293LevenbergMarquardtSpace::Status
294LevenbergMarquardt<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
342template<typename FunctorType>
343LevenbergMarquardtSpace::Status
344LevenbergMarquardt<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
365template<typename FunctorType>
366LevenbergMarquardtSpace::Status
367LevenbergMarquardt<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
Performs non linear optimization over a non-linear function, using a variant of the Levenberg Marquar...
Definition: LevenbergMarquardt/LevenbergMarquardt.h:111
FVectorType & diag()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:197
void setFtol(RealScalar ftol)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:161
void resetParameters()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:145
RealScalar gtol() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:185
RealScalar xtol() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:179
RealScalar factor() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:188
void setFactor(RealScalar factor)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:167
Index njev()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:206
JacobianType & matrixR()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:228
void setEpsilon(RealScalar epsfcn)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:170
Index maxfev() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:194
void setExternalScaling(bool value)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:176
JacobianType & jacobian()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:223
PermutationType permutation()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:232
void setXtol(RealScalar xtol)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:158
RealScalar ftol() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:182
FVectorType & fvec()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:219
RealScalar lm_param(void)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:215
RealScalar epsilon() const
Definition: LevenbergMarquardt/LevenbergMarquardt.h:191
RealScalar gnorm()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:212
RealScalar fnorm()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:209
void setGtol(RealScalar gtol)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:164
Index nfev()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:203
ComputationInfo info() const
Reports whether the minimization was successful.
Definition: LevenbergMarquardt/LevenbergMarquardt.h:243
Index iterations()
Definition: LevenbergMarquardt/LevenbergMarquardt.h:200
void setMaxfev(Index maxfev)
Definition: LevenbergMarquardt/LevenbergMarquardt.h:173
ComputationInfo
Namespace containing all symbols from the Eigen library.
const Eigen::CwiseUnaryOp< Eigen::internal::scalar_sqrt_op< typename Derived::Scalar >, const Derived > sqrt(const Eigen::ArrayBase< Derived > &x)
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
const int Dynamic