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-2017, 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/utils/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_base_grp
35  */
36 template <typename VECTORTYPE = Eigen::VectorXd, class USERPARAM = VECTORTYPE>
37 class CLevenbergMarquardtTempl : public mrpt::utils::COutputLogger
38 {
39  public:
40  typedef typename VECTORTYPE::Scalar NUMTYPE;
41  typedef Eigen::Matrix<NUMTYPE, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
42  typedef VECTORTYPE vector_t;
43 
45  : mrpt::utils::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,
106  mrpt::utils::VerbosityLevel verbosity = mrpt::utils::LVL_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::utils;
113  using namespace mrpt::math;
114  using namespace std;
115 
116  MRPT_START
117 
118  this->setMinLoggingLevel(verbosity);
119 
120  VECTORTYPE& x = out_optimal_x; // Var rename
121 
122  // Asserts:
123  ASSERT_(increments.size() == x0.size());
124 
125  x = x0; // Start with the starting point
126  VECTORTYPE f_x; // The vector error from the user function
127  matrix_t AUX;
128  matrix_t J; // The Jacobian of "f"
129  VECTORTYPE g; // The gradient
130 
131  // Compute the jacobian and the Hessian:
132  mrpt::math::estimateJacobian(x, functor, increments, userParam, J);
133  out_info.H.multiply_AtA(J);
134 
135  const size_t H_len = out_info.H.getColCount();
136 
137  // Compute the gradient:
138  functor(x, userParam, f_x);
139  J.multiply_Atb(f_x, g);
140 
141  // Start iterations:
142  bool found = math::norm_inf(g) <= e1;
143  if (found)
144  logFmt(
145  mrpt::utils::LVL_INFO,
146  "End condition: math::norm_inf(g)<=e1 :%f\n",
147  math::norm_inf(g));
148 
149  NUMTYPE lambda = tau * out_info.H.maximumDiagonal();
150  size_t iter = 0;
151  NUMTYPE v = 2;
152 
153  VECTORTYPE h_lm;
154  VECTORTYPE xnew, f_xnew;
155  NUMTYPE F_x = pow(math::norm(f_x), 2);
156 
157  const size_t N = x.size();
158 
159  if (returnPath)
160  {
161  out_info.path.setSize(maxIter, N + 1);
162  out_info.path.block(iter, 0, 1, N) = x.transpose();
163  }
164  else
165  out_info.path = Eigen::Matrix<NUMTYPE, Eigen::Dynamic,
166  Eigen::Dynamic>(); // Empty matrix
167 
168  while (!found && ++iter < maxIter)
169  {
170  // H_lm = -( H + \lambda I ) ^-1 * g
171  matrix_t H = out_info.H;
172  for (size_t k = 0; k < H_len; k++) H(k, k) += lambda;
173 
174  H.inv_fast(AUX);
175  AUX.multiply_Ab(g, h_lm);
176  h_lm *= NUMTYPE(-1.0);
177 
178  double h_lm_n2 = math::norm(h_lm);
179  double x_n2 = math::norm(x);
180 
181  logFmt(
182  mrpt::utils::LVL_DEBUG, "Iter:%u x=%s\n", (unsigned)iter,
183  sprintf_vector(" %f", x).c_str());
184 
185  if (h_lm_n2 < e2 * (x_n2 + e2))
186  {
187  // Done:
188  found = true;
189  logFmt(
190  mrpt::utils::LVL_INFO, "End condition: %e < %e\n", h_lm_n2,
191  e2 * (x_n2 + e2));
192  }
193  else
194  {
195  // Improvement: xnew = x + h_lm;
196  if (!x_increment_adder)
197  xnew = x + h_lm; // Normal Euclidean space addition.
198  else
199  x_increment_adder(xnew, x, h_lm, userParam);
200 
201  functor(xnew, userParam, f_xnew);
202  const double F_xnew = pow(math::norm(f_xnew), 2);
203 
204  // denom = h_lm^t * ( \lambda * h_lm - g )
205  VECTORTYPE tmp(h_lm);
206  tmp *= lambda;
207  tmp -= g;
208  tmp.array() *= h_lm.array();
209  double denom = tmp.sum();
210  double l = (F_x - F_xnew) / denom;
211 
212  if (l > 0) // There is an improvement:
213  {
214  // Accept new point:
215  x = xnew;
216  f_x = f_xnew;
217  F_x = F_xnew;
218 
220  x, functor, increments, userParam, J);
221  out_info.H.multiply_AtA(J);
222  J.multiply_Atb(f_x, g);
223 
224  found = math::norm_inf(g) <= e1;
225  if (found)
226  logFmt(
227  mrpt::utils::LVL_INFO,
228  "End condition: math::norm_inf(g)<=e1 : %e\n",
229  math::norm_inf(g));
230 
231  lambda *= max(0.33, 1 - pow(2 * l - 1, 3));
232  v = 2;
233  }
234  else
235  {
236  // Nope...
237  lambda *= v;
238  v *= 2;
239  }
240 
241  if (returnPath)
242  {
243  out_info.path.block(iter, 0, 1, x.size()) = x.transpose();
244  out_info.path(iter, x.size()) = F_x;
245  }
246  }
247  } // end while
248 
249  // Output info:
250  out_info.final_sqr_err = F_x;
251  out_info.iterations_executed = iter;
252  out_info.last_err_vector = f_x;
253  if (returnPath) out_info.path.setSize(iter, N + 1);
254 
255  MRPT_END
256  }
257 
258 }; // End of class def.
259 
260 /** The default name for the LM class is an instantiation for "double" */
262 
263 } // End of namespace
264 } // End of namespace
265 #endif
An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
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
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 execute(VECTORTYPE &out_optimal_x, const VECTORTYPE &x0, TFunctorEval functor, const VECTORTYPE &increments, const USERPARAM &userParam, TResultInfo &out_info, mrpt::utils::VerbosityLevel verbosity=mrpt::utils::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...
CLevenbergMarquardtTempl< mrpt::math::CVectorDouble > CLevenbergMarquardt
The default name for the LM class is an instantiation for "double".
Eigen::Matrix< NUMTYPE, Eigen::Dynamic, Eigen::Dynamic > matrix_t
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
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
#define MRPT_END
matrix_t path
Each row is the optimized value at each iteration.
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 ...
CONTAINER::Scalar norm_inf(const CONTAINER &v)
#define MRPT_START
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define ASSERT_(f)
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
double Scalar
Definition: KmUtils.h:44
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: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019