Main MRPT website > C++ reference for MRPT 1.5.7
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 
18 namespace mrpt
19 {
20 namespace math
21 {
22  /** An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
23  *
24  * Refer to this <a href="http://www.mrpt.org/Levenberg%E2%80%93Marquardt_algorithm" >page</a> for more details on the algorithm and its usage.
25  *
26  * \tparam NUMTYPE The numeric type for all the operations (float, double, or long double)
27  * \tparam USERPARAM The type of the "y" input to the user supplied evaluation functor. Default type is a vector of NUMTYPE.
28  * \ingroup mrpt_base_grp
29  */
30  template <typename VECTORTYPE = Eigen::VectorXd, class USERPARAM = VECTORTYPE >
31  class CLevenbergMarquardtTempl : public mrpt::utils::COutputLogger
32  {
33  public:
34  typedef typename VECTORTYPE::Scalar NUMTYPE;
35  typedef Eigen::Matrix<NUMTYPE,Eigen::Dynamic,Eigen::Dynamic> matrix_t;
36  typedef VECTORTYPE vector_t;
37 
39  mrpt::utils::COutputLogger("CLevenbergMarquardt")
40  {}
41 
42  /** The type of the function passed to execute. The user must supply a function which evaluates the error of a given point in the solution space.
43  * \param x The state point under examination.
44  * \param y The same object passed to "execute" as the parameter "userParam".
45  * \param out The vector of (non-squared) errors, of the average square root error, for the given "x". The functor code must set the size of this vector.
46  */
47  typedef void (*TFunctorEval)(
48  const VECTORTYPE &x,
49  const USERPARAM &y,
50  VECTORTYPE &out);
51 
52  /** The type of an optional functor passed to \a execute to replace the Euclidean addition "x_new = x_old + x_incr" by any other operation.
53  */
54  typedef void (*TFunctorIncrement)(
55  VECTORTYPE &x_new,
56  const VECTORTYPE &x_old,
57  const VECTORTYPE &x_incr,
58  const USERPARAM &user_param);
59 
60  struct TResultInfo
61  {
64  VECTORTYPE last_err_vector; //!< The last error vector returned by the user-provided functor.
65  matrix_t path; //!< Each row is the optimized value at each iteration.
66 
67  /** This matrix can be used to obtain an estimate of the optimal parameters covariance matrix:
68  * \f[ COV = H M H^\top \f]
69  * With COV the covariance matrix of the optimal parameters, H this matrix, and M the covariance of the input (observations).
70  */
72  };
73 
74  /** Executes the LM-method, with derivatives estimated from
75  * \a functor is a user-provided function which takes as input two vectors, in this order:
76  * - x: The parameters to be optimized.
77  * - userParam: The vector passed to the LM algorithm, unmodified.
78  * and must return the "error vector", or the error (not squared) in each measured dimension, so the sum of the square of that output is the overall square error.
79  *
80  * \a x_increment_adder Is an optional functor which may replace the Euclidean "x_new = x + x_increment" at the core of the incremental optimizer by
81  * any other operation. It can be used for example, in on-manifold optimizations.
82  */
83  void execute(
84  VECTORTYPE &out_optimal_x,
85  const VECTORTYPE &x0,
86  TFunctorEval functor,
87  const VECTORTYPE &increments,
88  const USERPARAM &userParam,
89  TResultInfo &out_info,
90  mrpt::utils::VerbosityLevel verbosity = mrpt::utils::LVL_INFO,
91  const size_t maxIter = 200,
92  const NUMTYPE tau = 1e-3,
93  const NUMTYPE e1 = 1e-8,
94  const NUMTYPE e2 = 1e-8,
95  bool returnPath =true,
96  TFunctorIncrement x_increment_adder = NULL
97  )
98  {
99  using namespace mrpt;
100  using namespace mrpt::utils;
101  using namespace mrpt::math;
102  using namespace std;
103 
104  MRPT_START
105 
106  this->setMinLoggingLevel(verbosity);
107 
108  VECTORTYPE &x=out_optimal_x; // Var rename
109 
110  // Asserts:
111  ASSERT_( increments.size() == x0.size() );
112 
113  x=x0; // Start with the starting point
114  VECTORTYPE f_x; // The vector error from the user function
115  matrix_t AUX;
116  matrix_t J; // The Jacobian of "f"
117  VECTORTYPE g; // The gradient
118 
119  // Compute the jacobian and the Hessian:
120  mrpt::math::estimateJacobian( x, functor, increments, userParam, J);
121  out_info.H.multiply_AtA(J);
122 
123  const size_t H_len = out_info.H.getColCount();
124 
125  // Compute the gradient:
126  functor(x, userParam ,f_x);
127  J.multiply_Atb(f_x, g);
128 
129  // Start iterations:
130  bool found = math::norm_inf(g)<=e1;
131  if (found) logFmt(mrpt::utils::LVL_INFO, "End condition: math::norm_inf(g)<=e1 :%f\n",math::norm_inf(g));
132 
133  NUMTYPE lambda = tau * out_info.H.maximumDiagonal();
134  size_t iter = 0;
135  NUMTYPE v = 2;
136 
137  VECTORTYPE h_lm;
138  VECTORTYPE xnew, f_xnew ;
139  NUMTYPE F_x = pow( math::norm( f_x ), 2);
140 
141  const size_t N = x.size();
142 
143  if (returnPath) {
144  out_info.path.setSize(maxIter,N+1);
145  out_info.path.block(iter,0,1,N) = x.transpose();
146  } else out_info.path = Eigen::Matrix<NUMTYPE,Eigen::Dynamic,Eigen::Dynamic>(); // Empty matrix
147 
148  while (!found && ++iter<maxIter)
149  {
150  // H_lm = -( H + \lambda I ) ^-1 * g
151  matrix_t H = out_info.H;
152  for (size_t k=0;k<H_len;k++)
153  H(k,k)+= lambda;
154 
155  H.inv_fast(AUX);
156  AUX.multiply_Ab(g,h_lm);
157  h_lm *= NUMTYPE(-1.0);
158 
159  double h_lm_n2 = math::norm(h_lm);
160  double x_n2 = math::norm(x);
161 
162  logFmt(mrpt::utils::LVL_DEBUG, "Iter:%u x=%s\n",(unsigned)iter,sprintf_vector(" %f",x).c_str() );
163 
164  if (h_lm_n2<e2*(x_n2+e2))
165  {
166  // Done:
167  found = true;
168  logFmt(mrpt::utils::LVL_INFO, "End condition: %e < %e\n", h_lm_n2, e2*(x_n2+e2) );
169  }
170  else
171  {
172  // Improvement: xnew = x + h_lm;
173  if (!x_increment_adder)
174  xnew = x + h_lm; // Normal Euclidean space addition.
175  else x_increment_adder(xnew, x, h_lm, userParam);
176 
177  functor(xnew, userParam ,f_xnew );
178  const double F_xnew = pow( math::norm(f_xnew), 2);
179 
180  // denom = h_lm^t * ( \lambda * h_lm - g )
181  VECTORTYPE tmp(h_lm);
182  tmp *= lambda;
183  tmp -= g;
184  tmp.array() *=h_lm.array();
185  double denom = tmp.sum();
186  double l = (F_x - F_xnew) / denom;
187 
188  if (l>0) // There is an improvement:
189  {
190  // Accept new point:
191  x = xnew;
192  f_x = f_xnew;
193  F_x = F_xnew;
194 
195  math::estimateJacobian( x, functor, increments, userParam, J);
196  out_info.H.multiply_AtA(J);
197  J.multiply_Atb(f_x, g);
198 
199  found = math::norm_inf(g)<=e1;
200  if (found) logFmt(mrpt::utils::LVL_INFO, "End condition: math::norm_inf(g)<=e1 : %e\n", math::norm_inf(g) );
201 
202  lambda *= max(0.33, 1-pow(2*l-1,3) );
203  v = 2;
204  }
205  else
206  {
207  // Nope...
208  lambda *= v;
209  v*= 2;
210  }
211 
212 
213  if (returnPath) {
214  out_info.path.block(iter,0,1,x.size()) = x.transpose();
215  out_info.path(iter,x.size()) = F_x;
216  }
217  }
218  } // end while
219 
220  // Output info:
221  out_info.final_sqr_err = F_x;
222  out_info.iterations_executed = iter;
223  out_info.last_err_vector = f_x;
224  if (returnPath) out_info.path.setSize(iter,N+1);
225 
226  MRPT_END
227  }
228 
229  }; // End of class def.
230 
231 
232  typedef CLevenbergMarquardtTempl<mrpt::math::CVectorDouble> CLevenbergMarquardt; //!< The default name for the LM class is an instantiation for "double"
233 
234  } // End of namespace
235 } // End of namespace
236 #endif
An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
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=NULL)
Executes the LM-method, with derivatives estimated from functor is a user-provided function which tak...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
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:25
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.
CLevenbergMarquardtTempl< mrpt::math::CVectorDouble > CLevenbergMarquardt
The default name for the LM class is an instantiation for "double".
STL namespace.
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:5575
Eigen::Matrix< NUMTYPE, Eigen::Dynamic, Eigen::Dynamic > matrix_t
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:3603
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void(* TFunctorEval)(const VECTORTYPE &x, const USERPARAM &y, VECTORTYPE &out)
The type of the function passed to execute.
#define ASSERT_(f)
void(* TFunctorIncrement)(VECTORTYPE &x_new, const VECTORTYPE &x_old, const VECTORTYPE &x_incr, const USERPARAM &user_param)
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:3516
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
void estimateJacobian(const VECTORLIKE &x, void(*functor)(const VECTORLIKE &x, const USERPARAM &y, VECTORLIKE3 &out), 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:26
GLenum GLint x
Definition: glext.h:3516
double Scalar
Definition: KmUtils.h:41
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019