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



Page generated by Doxygen 1.8.20 for MRPT 1.4.0 SVN: at Thu Aug 27 02:40:23 UTC 2020