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
16
17/*---------------------------------------------------------------
18 Class
19 ---------------------------------------------------------------*/
20namespace mrpt
21{
22namespace 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
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
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
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
An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
void(* TFunctorEval)(const VECTORTYPE &x, const USERPARAM &y, VECTORTYPE &out)
The type of the function passed to execute.
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 + ...
Eigen::Matrix< NUMTYPE, Eigen::Dynamic, Eigen::Dynamic > matrix_t
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...
This base class provides a common printf-like method to send debug information to std::cout,...
static void printf_debug(const char *frmt,...)
Sends a formated text to "debugOut" if not NULL, or to cout otherwise.
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERT_(f)
Definition: mrpt_macros.h:261
#define MRPT_END
Definition: mrpt_macros.h:353
This base provides a set of functions for maths stuff.
Definition: CArray.h:19
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
CONTAINER::Scalar norm_inf(const CONTAINER &v)
CONTAINER::Scalar norm(const CONTAINER &v)
CLevenbergMarquardtTempl< mrpt::math::CVectorDouble > CLevenbergMarquardt
The default name for the LM class is an instantiation for "double".
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: zip.h:16
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
STL namespace.
VECTORTYPE last_err_vector
The last error vector returned by the user-provided functor.
matrix_t H
This matrix can be used to obtain an estimate of the optimal parameters covariance matrix:
matrix_t path
Each row is the optimized value at each iteration.



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 02:56:26 UTC 2022