MRPT  1.9.9
CLevenbergMarquardt.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
14 #include <mrpt/math/num_jacobian.h>
17 #include <functional>
18 
19 namespace mrpt::math
20 {
21 /** An implementation of the Levenberg-Marquardt algorithm for least-square
22  * minimization.
23  *
24  * Refer to this <a
25  * href="http://www.mrpt.org/Levenberg%E2%80%93Marquardt_algorithm" >page</a>
26  * for more details on the algorithm and its usage.
27  *
28  * \tparam NUMTYPE The numeric type for all the operations (float, double, or
29  * long double)
30  * \tparam USERPARAM The type of the "y" input to the user supplied evaluation
31  * functor. Default type is a vector of NUMTYPE.
32  * \ingroup mrpt_math_grp
33  */
34 template <typename VECTORTYPE = CVectorDouble, class USERPARAM = VECTORTYPE>
36 {
37  public:
38  using NUMTYPE = typename VECTORTYPE::Scalar;
40  using vector_t = VECTORTYPE;
41 
43  : mrpt::system::COutputLogger("CLevenbergMarquardt")
44  {
45  }
46 
47  /** The type of the function passed to execute. The user must supply a
48  * function which evaluates the error of a given point in the solution
49  * space.
50  * \param x The state point under examination.
51  * \param y The same object passed to "execute" as the parameter
52  * "userParam".
53  * \param out The vector of (non-squared) errors, of the average square
54  * root error, for the given "x". The functor code must set the size of this
55  * vector.
56  */
57  using TFunctorEval = std::function<void(
58  const VECTORTYPE& x, const USERPARAM& y, VECTORTYPE& out)>;
59 
60  /** The type of an optional functor passed to \a execute to replace the
61  * Euclidean addition "x_new = x_old + x_incr" by any other operation.
62  */
63  using TFunctorIncrement = std::function<void(
64  VECTORTYPE& x_new, const VECTORTYPE& x_old, const VECTORTYPE& x_incr,
65  const USERPARAM& user_param)>;
66 
67  struct TResultInfo
68  {
71  /** The last error vector returned by the user-provided functor. */
72  VECTORTYPE last_err_vector;
73  /** Each row is the optimized value at each iteration. */
75 
76  /** This matrix can be used to obtain an estimate of the optimal
77  * parameters covariance matrix:
78  * \f[ COV = H M H^\top \f]
79  * With COV the covariance matrix of the optimal parameters, H this
80  * matrix, and M the covariance of the input (observations).
81  */
83  };
84 
85  /** Executes the LM-method, with derivatives estimated from
86  * \a functor is a user-provided function which takes as input two
87  *vectors, in this order:
88  * - x: The parameters to be optimized.
89  * - userParam: The vector passed to the LM algorithm, unmodified.
90  * and must return the "error vector", or the error (not squared) in each
91  *measured dimension, so the sum of the square of that output is the
92  *overall square error.
93  *
94  * \a x_increment_adder Is an optional functor which may replace the
95  *Euclidean "x_new = x + x_increment" at the core of the incremental
96  *optimizer by
97  * any other operation. It can be used for example, in on-manifold
98  *optimizations.
99  */
100  void execute(
101  VECTORTYPE& out_optimal_x, const VECTORTYPE& x0, TFunctorEval functor,
102  const VECTORTYPE& increments, const USERPARAM& userParam,
103  TResultInfo& out_info,
105  const size_t maxIter = 200, const NUMTYPE tau = 1e-3,
106  const NUMTYPE e1 = 1e-8, const NUMTYPE e2 = 1e-8,
107  bool returnPath = true, TFunctorIncrement x_increment_adder = nullptr)
108  {
109  using namespace mrpt;
110  using namespace mrpt::math;
111  using namespace std;
112 
113  MRPT_START
114 
115  this->setMinLoggingLevel(verbosity);
116 
117  VECTORTYPE& x = out_optimal_x; // Var rename
118 
119  // Asserts:
120  ASSERT_(increments.size() == x0.size());
121 
122  x = x0; // Start with the starting point
123  VECTORTYPE f_x; // The vector error from the user function
124  matrix_t AUX;
125  matrix_t J; // The Jacobian of "f"
126  VECTORTYPE g; // The gradient
127 
128  // Compute the jacobian and the Hessian:
129  mrpt::math::estimateJacobian(x, functor, increments, userParam, J);
130  out_info.H.matProductOf_AtA(J);
131 
132  const size_t H_len = out_info.H.cols();
133 
134  // Compute the gradient:
135  functor(x, userParam, f_x);
136  // g <- J' * f_x
137  g.matProductOf_Atb(J, f_x);
138 
139  // Start iterations:
140  bool found = math::norm_inf(g) <= e1;
141  if (found)
142  logFmt(
144  "End condition: math::norm_inf(g)<=e1 :%f\n",
145  math::norm_inf(g));
146 
147  NUMTYPE lambda = tau * out_info.H.maximumDiagonal();
148  size_t iter = 0;
149  NUMTYPE v = 2;
150 
151  VECTORTYPE h_lm;
152  VECTORTYPE xnew, f_xnew;
153  NUMTYPE F_x = pow(math::norm(f_x), 2);
154 
155  const size_t N = x.size();
156 
157  if (returnPath)
158  {
159  out_info.path.setSize(maxIter, N + 1);
160  for (size_t i = 0; i < N; i++) out_info.path(iter, i) = x[i];
161  }
162  else
163  out_info.path = matrix_t(); // Empty matrix
164 
165  while (!found && ++iter < maxIter)
166  {
167  // H_lm = -( H + \lambda I ) ^-1 * g
168  matrix_t H = out_info.H;
169  for (size_t k = 0; k < H_len; k++) H(k, k) += lambda;
170 
171  AUX = H.inverse_LLt();
172  // AUX.matProductOf_Ab(g,h_lm); h_lm <- AUX*g
173  h_lm.matProductOf_Ab(AUX, g);
174  h_lm *= NUMTYPE(-1.0);
175 
176  double h_lm_n2 = math::norm(h_lm);
177  double x_n2 = math::norm(x);
178 
180  "Iter:" << iter
181  << " x=" << mrpt::containers::sprintf_vector(" %f", x));
182 
183  if (h_lm_n2 < e2 * (x_n2 + e2))
184  {
185  // Done:
186  found = true;
187  logFmt(
188  mrpt::system::LVL_INFO, "End condition: %e < %e\n", h_lm_n2,
189  e2 * (x_n2 + e2));
190  }
191  else
192  {
193  // Improvement: xnew = x + h_lm;
194  if (!x_increment_adder)
195  xnew = x + h_lm; // Normal Euclidean space addition.
196  else
197  x_increment_adder(xnew, x, h_lm, userParam);
198 
199  functor(xnew, userParam, f_xnew);
200  const double F_xnew = pow(math::norm(f_xnew), 2);
201 
202  // denom = h_lm^t * ( \lambda * h_lm - g )
203  VECTORTYPE tmp(h_lm);
204  tmp *= lambda;
205  tmp -= g;
206  const double denom = tmp.dot(h_lm);
207  const double l = (F_x - F_xnew) / denom;
208 
209  if (l > 0) // There is an improvement:
210  {
211  // Accept new point:
212  x = xnew;
213  f_x = f_xnew;
214  F_x = F_xnew;
215 
217  x, functor, increments, userParam, J);
218  out_info.H.matProductOf_AtA(J);
219  g.matProductOf_Atb(J, f_x);
220 
221  found = math::norm_inf(g) <= e1;
222  if (found)
223  logFmt(
225  "End condition: math::norm_inf(g)<=e1 : %e\n",
226  math::norm_inf(g));
227 
228  lambda *= max(0.33, 1 - pow(2 * l - 1, 3));
229  v = 2;
230  }
231  else
232  {
233  // Nope...
234  lambda *= v;
235  v *= 2;
236  }
237 
238  if (returnPath)
239  {
240  for (size_t i = 0; i < N; i++)
241  out_info.path(iter, i) = x[i];
242  out_info.path(iter, x.size()) = F_x;
243  }
244  }
245  } // end while
246 
247  // Output info:
248  out_info.final_sqr_err = F_x;
249  out_info.iterations_executed = iter;
250  out_info.last_err_vector = f_x;
251  if (returnPath) out_info.path.setSize(iter, N + 1);
252 
253  MRPT_END
254  }
255 
256 }; // End of class def.
257 
258 /** The default name for the LM class is an instantiation for "double" */
260 
261 } // namespace mrpt::math
typename VECTORTYPE::Scalar NUMTYPE
An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
void matProductOf_AtA(const MAT_A &A)
this = AT * A
Definition: MatrixBase.h:295
double Scalar
Definition: KmUtils.h:43
#define MRPT_START
Definition: exceptions.h:241
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.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This base provides a set of functions for maths stuff.
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:6372
Scalar maximumDiagonal() const
Returns the maximum value in the diagonal.
matrix_t H
This matrix can be used to obtain an estimate of the optimal parameters covariance matrix: With COV ...
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
size_type cols() const
Number of columns in the matrix.
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
CONTAINER::Scalar norm_inf(const CONTAINER &v)
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
COutputLogger()
Default class constructor.
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
#define MRPT_END
Definition: exceptions.h:245
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:3542
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
GLenum GLint x
Definition: glext.h:3542
std::string sprintf_vector(const char *fmt, const VEC &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:24
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: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019