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 |
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  {
70  size_t iterations_executed = 0;
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
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
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  out_info.initial_sqr_err = F_x;
155
156  const size_t N = x.size();
157
158  if (returnPath)
159  {
160  out_info.path.setSize(maxIter, N + 1);
161  for (size_t i = 0; i < N; i++) out_info.path(iter, i) = x[i];
162  }
163  else
164  out_info.path = matrix_t(); // Empty matrix
165
166  while (!found && ++iter < maxIter)
167  {
168  // H_lm = -( H + \lambda I ) ^-1 * g
169  matrix_t H = out_info.H;
170  for (size_t k = 0; k < H_len; k++) H(k, k) += lambda;
171
172  AUX = H.inverse_LLt();
173  // AUX.matProductOf_Ab(g,h_lm); h_lm <- AUX*g
174  h_lm.matProductOf_Ab(AUX, g);
175  h_lm *= NUMTYPE(-1.0);
176
177  double h_lm_n2 = math::norm(h_lm);
178  double x_n2 = math::norm(x);
179
181  "Iter:" << iter
182  << " x=" << mrpt::containers::sprintf_vector(" %f", x));
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;
196  xnew = x + h_lm; // Normal Euclidean space addition.
197  else
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  const double denom = tmp.dot(h_lm);
208  const double l = (F_x - F_xnew) / denom;
209
210  if (l > 0) // There is an improvement:
211  {
212  // Accept new point:
213  x = xnew;
214  f_x = f_xnew;
215  F_x = F_xnew;
216
218  x, functor, increments, userParam, J);
219  out_info.H.matProductOf_AtA(J);
220  g.matProductOf_Atb(J, f_x);
221
222  found = math::norm_inf(g) <= e1;
223  if (found)
224  logFmt(
226  "End condition: math::norm_inf(g)<=e1 : %e\n",
227  math::norm_inf(g));
228
229  lambda *= max(0.33, 1 - pow(2 * l - 1, 3));
230  v = 2;
231  }
232  else
233  {
234  // Nope...
235  lambda *= v;
236  v *= 2;
237  }
238
239  if (returnPath)
240  {
241  for (size_t i = 0; i < N; i++)
242  out_info.path(iter, i) = x[i];
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 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.
mrpt::vision::TStereoCalibResults out
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: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019