Main MRPT website > C++ reference
MRPT logo
CLevenbergMarquardt.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (c) 2005-2013, Individual contributors, see AUTHORS file |
7  | Copyright (c) 2005-2013, MAPIR group, University of Malaga |
8  | Copyright (c) 2012-2013, University of Almeria |
9  | All rights reserved. |
10  | |
11  | Redistribution and use in source and binary forms, with or without |
12  | modification, are permitted provided that the following conditions are |
13  | met: |
14  | * Redistributions of source code must retain the above copyright |
15  | notice, this list of conditions and the following disclaimer. |
16  | * Redistributions in binary form must reproduce the above copyright |
17  | notice, this list of conditions and the following disclaimer in the |
18  | documentation and/or other materials provided with the distribution. |
19  | * Neither the name of the copyright holders nor the |
20  | names of its contributors may be used to endorse or promote products |
21  | derived from this software without specific prior written permission.|
22  | |
23  | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
24  | 'AS IS' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED |
25  | TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR|
26  | PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS BE LIABLE |
27  | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL|
28  | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR|
29  | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
30  | HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, |
31  | STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
32  | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
33  | POSSIBILITY OF SUCH DAMAGE. |
34  +---------------------------------------------------------------------------+ */
35 #ifndef CLevenbergMarquardt_H
36 #define CLevenbergMarquardt_H
37 
39 #include <mrpt/math/CMatrixD.h>
40 #include <mrpt/math/utils.h>
41 
42 /*---------------------------------------------------------------
43  Class
44  ---------------------------------------------------------------*/
45 namespace mrpt
46 {
47 namespace math
48 {
49  /** An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
50  *
51  * 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.
52  *
53  * \tparam NUMTYPE The numeric type for all the operations (float, double, or long double)
54  * \tparam USERPARAM The type of the "y" input to the user supplied evaluation functor. Default type is a vector of NUMTYPE.
55  * \ingroup mrpt_base_grp
56  */
57  template <typename VECTORTYPE = mrpt::vector_double, class USERPARAM = VECTORTYPE >
59  {
60  public:
61  typedef typename VECTORTYPE::value_type NUMTYPE;
63  typedef VECTORTYPE vector_t;
64 
65 
66  /** 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.
67  * \param x The state point under examination.
68  * \param y The same object passed to "execute" as the parameter "userParam".
69  * \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.
70  */
71  typedef void (*TFunctorEval)(
72  const VECTORTYPE &x,
73  const USERPARAM &y,
74  VECTORTYPE &out);
75 
76  /** 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.
77  */
78  typedef void (*TFunctorIncrement)(
79  VECTORTYPE &x_new,
80  const VECTORTYPE &x_old,
81  const VECTORTYPE &x_incr,
82  const USERPARAM &user_param);
83 
84  struct TResultInfo
85  {
88  VECTORTYPE last_err_vector; //!< The last error vector returned by the user-provided functor.
89  matrix_t path; //!< Each row is the optimized value at each iteration.
90 
91  /** This matrix can be used to obtain an estimate of the optimal parameters covariance matrix:
92  * \f[ COV = H M H^\top \f]
93  * With COV the covariance matrix of the optimal parameters, H this matrix, and M the covariance of the input (observations).
94  */
96  };
97 
98  /** Executes the LM-method, with derivatives estimated from
99  * \a functor is a user-provided function which takes as input two vectors, in this order:
100  * - x: The parameters to be optimized.
101  * - userParam: The vector passed to the LM algorithm, unmodified.
102  * 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.
103  *
104  * \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
105  * any other operation. It can be used for example, in on-manifold optimizations.
106  */
107  static void execute(
108  VECTORTYPE &out_optimal_x,
109  const VECTORTYPE &x0,
110  TFunctorEval functor,
111  const VECTORTYPE &increments,
112  const USERPARAM &userParam,
113  TResultInfo &out_info,
114  bool verbose = false,
115  const size_t maxIter = 200,
116  const NUMTYPE tau = 1e-3,
117  const NUMTYPE e1 = 1e-8,
118  const NUMTYPE e2 = 1e-8,
119  bool returnPath =true,
120  TFunctorIncrement x_increment_adder = NULL
121  )
122  {
123  using namespace mrpt;
124  using namespace mrpt::utils;
125  using namespace mrpt::math;
126  using namespace std;
127 
128  MRPT_START
129 
130  VECTORTYPE &x=out_optimal_x; // Var rename
131 
132  // Asserts:
133  ASSERT_( increments.size() == x0.size() );
134 
135  x=x0; // Start with the starting point
136  VECTORTYPE f_x; // The vector error from the user function
137  matrix_t AUX;
138  matrix_t J; // The Jacobian of "f"
139  VECTORTYPE g; // The gradient
140 
141  // Compute the jacobian and the Hessian:
142  mrpt::math::estimateJacobian( x, functor, increments, userParam, J);
143  out_info.H.multiply_AtA(J);
144 
145  const size_t H_len = out_info.H.getColCount();
146 
147  // Compute the gradient:
148  functor(x, userParam ,f_x);
149  J.multiply_Atb(f_x, g);
150 
151  // Start iterations:
152  bool found = math::norm_inf(g)<=e1;
153  if (verbose && found) printf_debug("[LM] End condition: math::norm_inf(g)<=e1 :%f\n",math::norm_inf(g));
154 
155  NUMTYPE lambda = tau * out_info.H.maximumDiagonal();
156  size_t iter = 0;
157  NUMTYPE v = 2;
158 
159  VECTORTYPE h_lm;
160  VECTORTYPE xnew, f_xnew ;
161  NUMTYPE F_x = pow( math::norm( f_x ), 2);
162 
163  const size_t N = x.size();
164 
165  if (returnPath) {
166  out_info.path.setSize(maxIter,N+1);
167  out_info.path.block(iter,0,1,N) = x.transpose();
168  } else out_info.path = Eigen::Matrix<NUMTYPE,Eigen::Dynamic,Eigen::Dynamic>(); // Empty matrix
169 
170  while (!found && ++iter<maxIter)
171  {
172  // H_lm = -( H + \lambda I ) ^-1 * g
173  matrix_t H = out_info.H;
174  for (size_t k=0;k<H_len;k++)
175  H(k,k)+= lambda;
176 
177  H.inv_fast(AUX);
178  AUX.multiply_Ab(g,h_lm);
179  h_lm *= NUMTYPE(-1.0);
180 
181  double h_lm_n2 = math::norm(h_lm);
182  double x_n2 = math::norm(x);
183 
184  if (verbose) printf_debug( (format("[LM] Iter: %u x:",(unsigned)iter)+ sprintf_vector(" %f",x) + string("\n")).c_str() );
185 
186  if (h_lm_n2<e2*(x_n2+e2))
187  {
188  // Done:
189  found = true;
190  if (verbose) printf_debug("[LM] End condition: %e < %e\n", h_lm_n2, 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 x_increment_adder(xnew, x, h_lm, userParam);
198 
199  functor(xnew, userParam ,f_xnew );
200  const double F_xnew = pow( math::norm(f_xnew), 2);
201 
202  // denom = h_lm^t * ( \lambda * h_lm - g )
203  VECTORTYPE tmp(h_lm);
204  tmp *= lambda;
205  tmp -= g;
206  tmp.array() *=h_lm.array();
207  double denom = tmp.sum();
208  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 
217  math::estimateJacobian( 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 (verbose && found) printf_debug("[LM] End condition: math::norm_inf(g)<=e1 : %e\n", math::norm_inf(g) );
223 
224  lambda *= max(0.33, 1-pow(2*l-1,3) );
225  v = 2;
226  }
227  else
228  {
229  // Nope...
230  lambda *= v;
231  v*= 2;
232  }
233 
234 
235  if (returnPath) {
236  out_info.path.block(iter,0,1,x.size()) = x.transpose();
237  out_info.path(iter,x.size()) = F_x;
238  }
239  }
240  } // end while
241 
242  // Output info:
243  out_info.final_sqr_err = F_x;
244  out_info.iterations_executed = iter;
245  out_info.last_err_vector = f_x;
246  if (returnPath) out_info.path.setSize(iter,N+1);
247 
248  MRPT_END
249  }
250 
251  }; // End of class def.
252 
253 
254  typedef CLevenbergMarquardtTempl<vector_double> CLevenbergMarquardt; //!< The default name for the LM class is an instantiation for "double"
255 
256  } // End of namespace
257 } // End of namespace
258 #endif
An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
mrpt::math::CMatrixTemplateNumeric< NUMTYPE > matrix_t
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...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:42
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
VECTORTYPE last_err_vector
The last error vector returned by the user-provided functor.
STL namespace.
This base provides a set of functions for maths stuff.
Definition: CArray.h:45
#define MRPT_END
matrix_t path
Each row is the optimized value at each iteration.
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...
matrix_t H
This matrix can be used to obtain an estimate of the optimal parameters covariance matrix: With COV ...
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...
CONTAINER::Scalar norm_inf(const CONTAINER &v)
#define MRPT_START
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)
CLevenbergMarquardtTempl< vector_double > CLevenbergMarquardt
The default name for the LM class is an instantiation for "double".
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 + ...
static void printf_debug(const char *frmt,...)
Sends a formated text to "debugOut" if not NULL, or to cout otherwise.
This base class provides a common printf-like method to send debug information to std::cout...
CONTAINER::Scalar norm(const CONTAINER &v)



Page generated by Doxygen 1.8.14 for MRPT 1.0.2 SVN: at lun oct 28 00:52:41 CET 2019 Hosted on:
SourceForge.net Logo