Main MRPT website > C++ reference for MRPT 1.9.9
CLevenbergMarquardt.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #ifndef CLevenbergMarquardt_H
10 #define CLevenbergMarquardt_H
11 
13 #include <mrpt/math/types_math.h>
14 #include <mrpt/math/num_jacobian.h>
17 #include <functional>
18 
19 namespace mrpt
20 {
21 namespace math
22 {
23 /** An implementation of the Levenberg-Marquardt algorithm for least-square
24  * minimization.
25  *
26  * Refer to this <a
27  * href="http://www.mrpt.org/Levenberg%E2%80%93Marquardt_algorithm" >page</a>
28  * for more details on the algorithm and its usage.
29  *
30  * \tparam NUMTYPE The numeric type for all the operations (float, double, or
31  * long double)
32  * \tparam USERPARAM The type of the "y" input to the user supplied evaluation
33  * functor. Default type is a vector of NUMTYPE.
34  * \ingroup mrpt_math_grp
35  */
36 template <typename VECTORTYPE = Eigen::VectorXd, class USERPARAM = VECTORTYPE>
38 {
39  public:
40  using NUMTYPE = typename VECTORTYPE::Scalar;
41  using matrix_t = Eigen::Matrix<NUMTYPE, Eigen::Dynamic, Eigen::Dynamic>;
42  using vector_t = VECTORTYPE;
43 
45  : mrpt::system::COutputLogger("CLevenbergMarquardt")
46  {
47  }
48 
49  /** The type of the function passed to execute. The user must supply a
50  * function which evaluates the error of a given point in the solution
51  * space.
52  * \param x The state point under examination.
53  * \param y The same object passed to "execute" as the parameter
54  * "userParam".
55  * \param out The vector of (non-squared) errors, of the average square
56  * root error, for the given "x". The functor code must set the size of this
57  * vector.
58  */
59  using TFunctorEval = std::function<void(
60  const VECTORTYPE& x, const USERPARAM& y, VECTORTYPE& out)>;
61 
62  /** The type of an optional functor passed to \a execute to replace the
63  * Euclidean addition "x_new = x_old + x_incr" by any other operation.
64  */
65  using TFunctorIncrement = std::function<void(
66  VECTORTYPE& x_new, const VECTORTYPE& x_old, const VECTORTYPE& x_incr,
67  const USERPARAM& user_param)>;
68 
69  struct TResultInfo
70  {
73  /** The last error vector returned by the user-provided functor. */
74  VECTORTYPE last_err_vector;
75  /** Each row is the optimized value at each iteration. */
77 
78  /** This matrix can be used to obtain an estimate of the optimal
79  * parameters covariance matrix:
80  * \f[ COV = H M H^\top \f]
81  * With COV the covariance matrix of the optimal parameters, H this
82  * matrix, and M the covariance of the input (observations).
83  */
85  };
86 
87  /** Executes the LM-method, with derivatives estimated from
88  * \a functor is a user-provided function which takes as input two
89  *vectors, in this order:
90  * - x: The parameters to be optimized.
91  * - userParam: The vector passed to the LM algorithm, unmodified.
92  * and must return the "error vector", or the error (not squared) in each
93  *measured dimension, so the sum of the square of that output is the
94  *overall square error.
95  *
96  * \a x_increment_adder Is an optional functor which may replace the
97  *Euclidean "x_new = x + x_increment" at the core of the incremental
98  *optimizer by
99  * any other operation. It can be used for example, in on-manifold
100  *optimizations.
101  */
102  void execute(
103  VECTORTYPE& out_optimal_x, const VECTORTYPE& x0, TFunctorEval functor,
104  const VECTORTYPE& increments, const USERPARAM& userParam,
105  TResultInfo& out_info,
107  const size_t maxIter = 200, const NUMTYPE tau = 1e-3,
108  const NUMTYPE e1 = 1e-8, const NUMTYPE e2 = 1e-8,
109  bool returnPath = true, TFunctorIncrement x_increment_adder = nullptr)
110  {
111  using namespace mrpt;
112  using namespace mrpt::math;
113  using namespace std;
114 
115  MRPT_START
116 
117  this->setMinLoggingLevel(verbosity);
118 
119  VECTORTYPE& x = out_optimal_x; // Var rename
120 
121  // Asserts:
122  ASSERT_(increments.size() == x0.size());
123 
124  x = x0; // Start with the starting point
125  VECTORTYPE f_x; // The vector error from the user function
126  matrix_t AUX;
127  matrix_t J; // The Jacobian of "f"
128  VECTORTYPE g; // The gradient
129 
130  // Compute the jacobian and the Hessian:
131  mrpt::math::estimateJacobian(x, functor, increments, userParam, J);
132  out_info.H.multiply_AtA(J);
133 
134  const size_t H_len = out_info.H.cols();
135 
136  // Compute the gradient:
137  functor(x, userParam, f_x);
138  J.multiply_Atb(f_x, g);
139 
140  // Start iterations:
141  bool found = math::norm_inf(g) <= e1;
142  if (found)
143  logFmt(
145  "End condition: math::norm_inf(g)<=e1 :%f\n",
146  math::norm_inf(g));
147 
148  NUMTYPE lambda = tau * out_info.H.maximumDiagonal();
149  size_t iter = 0;
150  NUMTYPE v = 2;
151 
152  VECTORTYPE h_lm;
153  VECTORTYPE xnew, f_xnew;
154  NUMTYPE F_x = pow(math::norm(f_x), 2);
155 
156  const size_t N = x.size();
157 
158  if (returnPath)
159  {
160  out_info.path.setSize(maxIter, N + 1);
161  out_info.path.block(iter, 0, 1, N) = x.transpose();
162  }
163  else
164  out_info.path = Eigen::Matrix<NUMTYPE, Eigen::Dynamic,
165  Eigen::Dynamic>(); // Empty matrix
166 
167  while (!found && ++iter < maxIter)
168  {
169  // H_lm = -( H + \lambda I ) ^-1 * g
170  matrix_t H = out_info.H;
171  for (size_t k = 0; k < H_len; k++) H(k, k) += lambda;
172 
173  H.inv_fast(AUX);
174  AUX.multiply_Ab(g, h_lm);
175  h_lm *= NUMTYPE(-1.0);
176 
177  double h_lm_n2 = math::norm(h_lm);
178  double x_n2 = math::norm(x);
179 
180  logFmt(
181  mrpt::system::LVL_DEBUG, "Iter:%u x=%s\n", (unsigned)iter,
182  mrpt::containers::sprintf_vector(" %f", x).c_str());
183 
184  if (h_lm_n2 < e2 * (x_n2 + e2))
185  {
186  // Done:
187  found = true;
188  logFmt(
189  mrpt::system::LVL_INFO, "End condition: %e < %e\n", h_lm_n2,
190  e2 * (x_n2 + e2));
191  }
192  else
193  {
194  // Improvement: xnew = x + h_lm;
195  if (!x_increment_adder)
196  xnew = x + h_lm; // Normal Euclidean space addition.
197  else
198  x_increment_adder(xnew, x, h_lm, userParam);
199 
200  functor(xnew, userParam, f_xnew);
201  const double F_xnew = pow(math::norm(f_xnew), 2);
202 
203  // denom = h_lm^t * ( \lambda * h_lm - g )
204  VECTORTYPE tmp(h_lm);
205  tmp *= lambda;
206  tmp -= g;
207  tmp.array() *= h_lm.array();
208  double denom = tmp.sum();
209  double l = (F_x - F_xnew) / denom;
210 
211  if (l > 0) // There is an improvement:
212  {
213  // Accept new point:
214  x = xnew;
215  f_x = f_xnew;
216  F_x = F_xnew;
217 
219  x, functor, increments, userParam, J);
220  out_info.H.multiply_AtA(J);
221  J.multiply_Atb(f_x, g);
222 
223  found = math::norm_inf(g) <= e1;
224  if (found)
225  logFmt(
227  "End condition: math::norm_inf(g)<=e1 : %e\n",
228  math::norm_inf(g));
229 
230  lambda *= max(0.33, 1 - pow(2 * l - 1, 3));
231  v = 2;
232  }
233  else
234  {
235  // Nope...
236  lambda *= v;
237  v *= 2;
238  }
239 
240  if (returnPath)
241  {
242  out_info.path.block(iter, 0, 1, x.size()) = x.transpose();
243  out_info.path(iter, x.size()) = F_x;
244  }
245  }
246  } // end while
247 
248  // Output info:
249  out_info.final_sqr_err = F_x;
250  out_info.iterations_executed = iter;
251  out_info.last_err_vector = f_x;
252  if (returnPath) out_info.path.setSize(iter, N + 1);
253 
254  MRPT_END
255  }
256 
257 }; // End of class def.
258 
259 /** The default name for the LM class is an instantiation for "double" */
261 
262 } // namespace math
263 } // namespace mrpt
264 #endif
typename VECTORTYPE::Scalar NUMTYPE
An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
double Scalar
Definition: KmUtils.h:44
#define MRPT_START
Definition: exceptions.h:262
void execute(VECTORTYPE &out_optimal_x, const VECTORTYPE &x0, TFunctorEval functor, const VECTORTYPE &increments, const USERPARAM &userParam, TResultInfo &out_info, mrpt::system::VerbosityLevel verbosity=mrpt::system::LVL_INFO, const size_t maxIter=200, const NUMTYPE tau=1e-3, const NUMTYPE e1=1e-8, const NUMTYPE e2=1e-8, bool returnPath=true, TFunctorIncrement x_increment_adder=nullptr)
Executes the LM-method, with derivatives estimated from functor is a user-provided function which tak...
VerbosityLevel
Enumeration of available verbosity levels.
void logFmt(const VerbosityLevel level, const char *fmt,...) const MRPT_printf_format_check(3
Alternative logging method, which mimics the printf behavior.
This file implements several operations that operate element-wise on individual or pairs of container...
VECTORTYPE last_err_vector
The last error vector returned by the user-provided functor.
void setMinLoggingLevel(const VerbosityLevel level)
Set the minimum logging level for which the incoming logs are going to be taken into account...
STL namespace.
void estimateJacobian(const VECTORLIKE &x, const std::function< void(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out)> &functor, const VECTORLIKE2 &increments, const USERPARAM &userParam, MATRIXLIKE &out_Jacobian)
Estimate the Jacobian of a multi-dimensional function around a point "x", using finite differences of...
Definition: num_jacobian.h:31
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
Eigen::Matrix< NUMTYPE, Eigen::Dynamic, Eigen::Dynamic > matrix_t
matrix_t path
Each row is the optimized value at each iteration.
Versatile class for consistent logging and management of output messages.
GLubyte g
Definition: glext.h:6279
matrix_t H
This matrix can be used to obtain an estimate of the optimal parameters covariance matrix: With COV ...
std::string sprintf_vector(const char *fmt, const std::vector< T > &V)
Generates a string for a vector in the format [A,B,C,...] to std::cout, and the fmt string for each v...
Definition: printf_vector.h:26
CONTAINER::Scalar norm_inf(const CONTAINER &v)
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
COutputLogger()
Default class constructor.
#define MRPT_END
Definition: exceptions.h:266
std::function< void(VECTORTYPE &x_new, const VECTORTYPE &x_old, const VECTORTYPE &x_incr, const USERPARAM &user_param)> TFunctorIncrement
The type of an optional functor passed to execute to replace the Euclidean addition "x_new = x_old + ...
GLenum GLint GLint y
Definition: glext.h:3538
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
GLenum GLint x
Definition: glext.h:3538
std::function< void(const VECTORTYPE &x, const USERPARAM &y, VECTORTYPE &out)> TFunctorEval
The type of the function passed to execute.
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019