Main MRPT website > C++ reference
MRPT logo
CLevenbergMarquardt.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CLevenbergMarquardt_H
29 #define CLevenbergMarquardt_H
30 
31 #include <mrpt/utils/CDebugOutputCapable.h>
32 #include <mrpt/math/CMatrixD.h>
33 #include <mrpt/math/utils.h>
34 
35 /*---------------------------------------------------------------
36  Class
37  ---------------------------------------------------------------*/
38 namespace mrpt
39 {
40 namespace math
41 {
42  /** An implementation of the Levenberg-Marquardt algorithm for least-square minimization.
43  *
44  * 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.
45  *
46  * \tparam NUMTYPE The numeric type for all the operations (float, double, or long double)
47  * \tparam USERPARAM The type of the "y" input to the user supplied evaluation functor. Default type is a vector of NUMTYPE.
48  * \ingroup mrpt_base_grp
49  */
50  template <typename VECTORTYPE = mrpt::vector_double, class USERPARAM = VECTORTYPE >
51  class CLevenbergMarquardtTempl : public mrpt::utils::CDebugOutputCapable
52  {
53  public:
54  typedef typename VECTORTYPE::value_type NUMTYPE;
55  typedef mrpt::math::CMatrixTemplateNumeric<NUMTYPE> matrix_t;
56  typedef VECTORTYPE vector_t;
57 
58 
59  /** 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.
60  * \param x The state point under examination.
61  * \param y The same object passed to "execute" as the parameter "userParam".
62  * \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.
63  */
64  typedef void (*TFunctorEval)(
65  const VECTORTYPE &x,
66  const USERPARAM &y,
67  VECTORTYPE &out);
68 
69  /** 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.
70  */
71  typedef void (*TFunctorIncrement)(
72  VECTORTYPE &x_new,
73  const VECTORTYPE &x_old,
74  const VECTORTYPE &x_incr,
75  const USERPARAM &user_param);
76 
77  struct TResultInfo
78  {
79  NUMTYPE final_sqr_err;
80  size_t iterations_executed;
81  VECTORTYPE last_err_vector; //!< The last error vector returned by the user-provided functor.
82  matrix_t path; //!< Each row is the optimized value at each iteration.
83 
84  /** This matrix can be used to obtain an estimate of the optimal parameters covariance matrix:
85  * \f[ COV = H M H^\top \f]
86  * With COV the covariance matrix of the optimal parameters, H this matrix, and M the covariance of the input (observations).
87  */
88  matrix_t H;
89  };
90 
91  /** Executes the LM-method, with derivatives estimated from
92  * \a functor is a user-provided function which takes as input two vectors, in this order:
93  * - x: The parameters to be optimized.
94  * - userParam: The vector passed to the LM algorithm, unmodified.
95  * 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.
96  *
97  * \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
98  * any other operation. It can be used for example, in on-manifold optimizations.
99  */
100  static void execute(
101  VECTORTYPE &out_optimal_x,
102  const VECTORTYPE &x0,
103  TFunctorEval functor,
104  const VECTORTYPE &increments,
105  const USERPARAM &userParam,
106  TResultInfo &out_info,
107  bool verbose = false,
108  const size_t maxIter = 200,
109  const NUMTYPE tau = 1e-3,
110  const NUMTYPE e1 = 1e-8,
111  const NUMTYPE e2 = 1e-8,
112  bool returnPath =true,
113  TFunctorIncrement x_increment_adder = NULL
114  )
115  {
116  using namespace mrpt;
117  using namespace mrpt::utils;
118  using namespace mrpt::math;
119  using namespace std;
120 
121  MRPT_START
122 
123  VECTORTYPE &x=out_optimal_x; // Var rename
124 
125  // Asserts:
126  ASSERT_( increments.size() == x0.size() );
127 
128  x=x0; // Start with the starting point
129  VECTORTYPE f_x; // The vector error from the user function
130  matrix_t AUX;
131  matrix_t J; // The Jacobian of "f"
132  VECTORTYPE g; // The gradient
133 
134  // Compute the jacobian and the Hessian:
135  mrpt::math::estimateJacobian( x, functor, increments, userParam, J);
136  out_info.H.multiply_AtA(J);
137 
138  const size_t H_len = out_info.H.getColCount();
139 
140  // Compute the gradient:
141  functor(x, userParam ,f_x);
142  J.multiply_Atb(f_x, g);
143 
144  // Start iterations:
145  bool found = math::norm_inf(g)<=e1;
146  if (verbose && found) printf_debug("[LM] End condition: math::norm_inf(g)<=e1 :%f\n",math::norm_inf(g));
147 
148  NUMTYPE lambda = tau * out_info.H.maximumDiagonal();
149  size_t iter = 0;
150  NUMTYPE v = 2;
151 
152  VECTORTYPE h_lm;
153  VECTORTYPE xnew, f_xnew ;
154  NUMTYPE F_x = pow( math::norm( f_x ), 2);
155 
156  const size_t N = x.size();
157 
158  if (returnPath) {
159  out_info.path.setSize(maxIter,N+1);
160  out_info.path.block(iter,0,1,N) = x.transpose();
161  } else out_info.path = Eigen::Matrix<NUMTYPE,Eigen::Dynamic,Eigen::Dynamic>(); // Empty matrix
162 
163  while (!found && ++iter<maxIter)
164  {
165  // H_lm = -( H + \lambda I ) ^-1 * g
166  matrix_t H = out_info.H;
167  for (size_t k=0;k<H_len;k++)
168  H(k,k)+= lambda;
169 
170  H.inv_fast(AUX);
171  AUX.multiply_Ab(g,h_lm);
172  h_lm *= NUMTYPE(-1.0);
173 
174  double h_lm_n2 = math::norm(h_lm);
175  double x_n2 = math::norm(x);
176 
177  if (verbose) printf_debug( (format("[LM] Iter: %u x:",(unsigned)iter)+ sprintf_vector(" %f",x) + string("\n")).c_str() );
178 
179  if (h_lm_n2<e2*(x_n2+e2))
180  {
181  // Done:
182  found = true;
183  if (verbose) printf_debug("[LM] End condition: %e < %e\n", h_lm_n2, e2*(x_n2+e2) );
184  }
185  else
186  {
187  // Improvement: xnew = x + h_lm;
188  if (!x_increment_adder)
189  xnew = x + h_lm; // Normal Euclidean space addition.
190  else x_increment_adder(xnew, x, h_lm, userParam);
191 
192  functor(xnew, userParam ,f_xnew );
193  const double F_xnew = pow( math::norm(f_xnew), 2);
194 
195  // denom = h_lm^t * ( \lambda * h_lm - g )
196  VECTORTYPE tmp(h_lm);
197  tmp *= lambda;
198  tmp -= g;
199  tmp.array() *=h_lm.array();
200  double denom = tmp.sum();
201  double l = (F_x - F_xnew) / denom;
202 
203  if (l>0) // There is an improvement:
204  {
205  // Accept new point:
206  x = xnew;
207  f_x = f_xnew;
208  F_x = F_xnew;
209 
210  math::estimateJacobian( x, functor, increments, userParam, J);
211  out_info.H.multiply_AtA(J);
212  J.multiply_Atb(f_x, g);
213 
214  found = math::norm_inf(g)<=e1;
215  if (verbose && found) printf_debug("[LM] End condition: math::norm_inf(g)<=e1 : %e\n", math::norm_inf(g) );
216 
217  lambda *= max(0.33, 1-pow(2*l-1,3) );
218  v = 2;
219  }
220  else
221  {
222  // Nope...
223  lambda *= v;
224  v*= 2;
225  }
226 
227 
228  if (returnPath) {
229  out_info.path.block(iter,0,1,x.size()) = x.transpose();
230  out_info.path(iter,x.size()) = F_x;
231  }
232  }
233  } // end while
234 
235  // Output info:
236  out_info.final_sqr_err = F_x;
237  out_info.iterations_executed = iter;
238  out_info.last_err_vector = f_x;
239  if (returnPath) out_info.path.setSize(iter,N+1);
240 
241  MRPT_END
242  }
243 
244  }; // End of class def.
245 
246 
247  typedef CLevenbergMarquardtTempl<vector_double> CLevenbergMarquardt; //!< The default name for the LM class is an instantiation for "double"
248 
249  } // End of namespace
250 } // End of namespace
251 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013