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  {
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 
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  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;
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  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.
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)
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 + ...
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: 24b95e159 Thu Jan 23 01:15:46 2020 +0100 at jue ene 23 01:30:10 CET 2020