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 |
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::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 = Eigen::VectorXd, class USERPARAM = VECTORTYPE>
36 {
37  public:
38  using NUMTYPE = typename VECTORTYPE::Scalar;
39  using matrix_t = Eigen::Matrix<NUMTYPE, Eigen::Dynamic, Eigen::Dynamic>;
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
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.multiply_AtA(J);
131
132  const size_t H_len = out_info.H.cols();
133
135  functor(x, userParam, f_x);
136  J.multiply_Atb(f_x, g);
137
138  // Start iterations:
139  bool found = math::norm_inf(g) <= e1;
140  if (found)
141  logFmt(
143  "End condition: math::norm_inf(g)<=e1 :%f\n",
144  math::norm_inf(g));
145
146  NUMTYPE lambda = tau * out_info.H.maximumDiagonal();
147  size_t iter = 0;
148  NUMTYPE v = 2;
149
150  VECTORTYPE h_lm;
151  VECTORTYPE xnew, f_xnew;
152  NUMTYPE F_x = pow(math::norm(f_x), 2);
153
154  const size_t N = x.size();
155
156  if (returnPath)
157  {
158  out_info.path.setSize(maxIter, N + 1);
159  out_info.path.block(iter, 0, 1, N) = x.transpose();
160  }
161  else
162  out_info.path = Eigen::Matrix<NUMTYPE, Eigen::Dynamic,
163  Eigen::Dynamic>(); // 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  H.inv_fast(AUX);
172  AUX.multiply_Ab(g, h_lm);
173  h_lm *= NUMTYPE(-1.0);
174
175  double h_lm_n2 = math::norm(h_lm);
176  double x_n2 = math::norm(x);
177
178  logFmt(
179  mrpt::system::LVL_DEBUG, "Iter:%u x=%s\n", (unsigned)iter,
180  mrpt::containers::sprintf_vector(" %f", x).c_str());
181
182  if (h_lm_n2 < e2 * (x_n2 + e2))
183  {
184  // Done:
185  found = true;
186  logFmt(
187  mrpt::system::LVL_INFO, "End condition: %e < %e\n", h_lm_n2,
188  e2 * (x_n2 + e2));
189  }
190  else
191  {
192  // Improvement: xnew = x + h_lm;
194  xnew = x + h_lm; // Normal Euclidean space addition.
195  else
197
198  functor(xnew, userParam, f_xnew);
199  const double F_xnew = pow(math::norm(f_xnew), 2);
200
201  // denom = h_lm^t * ( \lambda * h_lm - g )
202  VECTORTYPE tmp(h_lm);
203  tmp *= lambda;
204  tmp -= g;
205  tmp.array() *= h_lm.array();
206  double denom = tmp.sum();
207  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.multiply_AtA(J);
219  J.multiply_Atb(f_x, g);
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  out_info.path.block(iter, 0, 1, x.size()) = x.transpose();
241  out_info.path(iter, x.size()) = F_x;
242  }
243  }
244  } // end while
245
246  // Output info:
247  out_info.final_sqr_err = F_x;
248  out_info.iterations_executed = iter;
249  out_info.last_err_vector = f_x;
250  if (returnPath) out_info.path.setSize(iter, N + 1);
251
252  MRPT_END
253  }
254
255 }; // End of class def.
256
257 /** The default name for the LM class is an instantiation for "double" */
259
260 }
261 #endif
262
263
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:29
#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:24
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020