9#ifndef CLevenbergMarquardt_H
10#define CLevenbergMarquardt_H
32 template <
typename VECTORTYPE = Eigen::VectorXd,
class USERPARAM = VECTORTYPE >
36 typedef typename VECTORTYPE::Scalar
NUMTYPE;
37 typedef Eigen::Matrix<NUMTYPE,Eigen::Dynamic,Eigen::Dynamic>
matrix_t;
55 const VECTORTYPE &x_old,
56 const VECTORTYPE &x_incr,
57 const USERPARAM &user_param);
83 VECTORTYPE &out_optimal_x,
86 const VECTORTYPE &increments,
87 const USERPARAM &userParam,
90 const size_t maxIter = 200,
94 bool returnPath =
true,
105 VECTORTYPE &x=out_optimal_x;
108 ASSERT_( increments.size() == x0.size() );
118 out_info.
H.multiply_AtA(J);
120 const size_t H_len = out_info.
H.getColCount();
123 functor(x, userParam ,f_x);
124 J.multiply_Atb(f_x, g);
130 NUMTYPE lambda = tau * out_info.
H.maximumDiagonal();
135 VECTORTYPE xnew, f_xnew ;
138 const size_t N = x.size();
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>();
145 while (!found && ++iter<maxIter)
149 for (
size_t k=0;k<H_len;k++)
153 AUX.multiply_Ab(g,h_lm);
159 if (verbose)
printf_debug( (
format(
"[LM] Iter: %u x:",(
unsigned)iter)+ sprintf_vector(
" %f",x) + std::string(
"\n")).c_str() );
161 if (h_lm_n2<e2*(x_n2+e2))
165 if (verbose)
printf_debug(
"[LM] End condition: %e < %e\n", h_lm_n2, e2*(x_n2+e2) );
170 if (!x_increment_adder)
172 else x_increment_adder(xnew, x, h_lm, userParam);
174 functor(xnew, userParam ,f_xnew );
175 const double F_xnew = pow(
math::norm(f_xnew), 2);
178 VECTORTYPE tmp(h_lm);
181 tmp.array() *=h_lm.array();
182 double denom = tmp.sum();
183 double l = (F_x - F_xnew) / denom;
193 out_info.
H.multiply_AtA(J);
194 J.multiply_Atb(f_x, g);
199 lambda *= max(0.33, 1-pow(2*l-1,3) );
211 out_info.
path.block(iter,0,1,x.size()) = x.transpose();
212 out_info.
path(iter,x.size()) = F_x;
221 if (returnPath) out_info.
path.setSize(iter,N+1);
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.
VECTORTYPE::Scalar NUMTYPE
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.
This base provides a set of functions for maths stuff.
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)
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,...
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.
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:
size_t iterations_executed
matrix_t path
Each row is the optimized value at each iteration.