Main MRPT website > C++ reference for MRPT 1.5.5
levmarq.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-2017, 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 GRAPH_SLAM_LEVMARQ_H
10 #define GRAPH_SLAM_LEVMARQ_H
11 
12 #include <mrpt/graphslam/types.h>
13 #include <mrpt/utils/TParameters.h>
14 #include <mrpt/utils/stl_containers_utils.h> // find_in_vector()
15 #include <mrpt/graphslam/levmarq_impl.h> // Aux classes
16 
17 #include <iterator> // ostream_iterator
18 
19 namespace mrpt
20 {
21  namespace graphslam
22  {
23  /** Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and a Levenberg-Marquardt optimizer.
24  * This method works for all types of graphs derived from \a CNetworkOfPoses (see its reference mrpt::graphs::CNetworkOfPoses for the list).
25  * The input data are all the pose constraints in \a graph (graph.edges), and the gross first estimations of the "global" pose coordinates (in graph.nodes).
26  *
27  * Note that these first coordinates can be obtained with mrpt::graphs::CNetworkOfPoses::dijkstra_nodes_estimate().
28  *
29  * The method implemented in this file is based on this work:
30  * - "Efficient Sparse Pose Adjustment for 2D Mapping", Kurt Konolige et al., 2010.
31  * , but generalized for not only 2D but 2D and 3D poses, and using on-manifold optimization.
32  *
33  * \param[in,out] graph The input edges and output poses.
34  * \param[out] out_info Some basic output information on the process.
35  * \param[in] nodes_to_optimize The list of nodes whose global poses are to be optimized. If NULL (default), all the node IDs are optimized (but that marked as \a root in the graph).
36  * \param[in] extra_params Optional parameters, see below.
37  * \param[in] functor_feedback Optional: a pointer to a user function can be set here to be called on each LM loop iteration (eg to refresh the current state and error, refresh a GUI, etc.)
38  *
39  * List of optional parameters by name in "extra_params":
40  * - "verbose": (default=0) If !=0, produce verbose ouput.
41  * - "max_iterations": (default=100) Maximum number of Lev-Marq. iterations.
42  * - "scale_hessian": (default=0.1) Multiplies the Hessian matrix by this scalar (may improve convergence speed).
43  * - "initial_lambda": (default=0) <=0 means auto guess, otherwise, initial lambda value for the lev-marq algorithm.
44  * - "tau": (default=1e-3) Initial tau value for the lev-marq algorithm.
45  * - "e1": (default=1e-6) Lev-marq algorithm iteration stopping criterion #1: |gradient| < e1
46  * - "e2": (default=1e-6) Lev-marq algorithm iteration stopping criterion #2: |delta_incr| < e2*(x_norm+e2)
47  *
48  * \note The following graph types are supported: mrpt::graphs::CNetworkOfPoses2D, mrpt::graphs::CNetworkOfPoses3D, mrpt::graphs::CNetworkOfPoses2DInf, mrpt::graphs::CNetworkOfPoses3DInf
49  *
50  * \tparam GRAPH_T Normally a mrpt::graphs::CNetworkOfPoses<EDGE_TYPE,MAPS_IMPLEMENTATION>. Users won't have to write this template argument by hand, since the compiler will auto-fit it depending on the type of the graph object.
51  * \sa The example "graph_slam_demo"
52  * \ingroup mrpt_graphslam_grp
53  * \note Implementation can be found in file \a levmarq_impl.h
54  */
55  template <class GRAPH_T>
56  void optimize_graph_spa_levmarq(
57  GRAPH_T & graph,
58  TResultInfoSpaLevMarq & out_info,
59  const std::set<mrpt::utils::TNodeID> * in_nodes_to_optimize = NULL,
60  const mrpt::utils::TParametersDouble & extra_params = mrpt::utils::TParametersDouble(),
61  typename graphslam_traits<GRAPH_T>::TFunctorFeedback functor_feedback = NULL
62  )
63  {
64  using namespace mrpt;
65  using namespace mrpt::poses;
66  using namespace mrpt::graphslam;
67  using namespace mrpt::math;
68  using namespace mrpt::utils;
69  using namespace std;
70 
71  MRPT_START
72 
73  // Typedefs to make life easier:
74  typedef graphslam_traits<GRAPH_T> gst;
75 
76  typename gst::Array_O array_O_zeros; array_O_zeros.fill(0); // Auxiliary var with all zeros
77 
78  // The size of things here (because size matters...)
79  static const unsigned int DIMS_POSE = gst::SE_TYPE::VECTOR_SIZE;
80 
81  // Read extra params:
82  const bool verbose = 0!=extra_params.getWithDefaultVal("verbose",0);
83  const size_t max_iters = extra_params.getWithDefaultVal("max_iterations",100);
84  const bool enable_profiler = 0!=extra_params.getWithDefaultVal("profiler",0);
85  // LM params:
86  const double initial_lambda = extra_params.getWithDefaultVal("initial_lambda", 0); // <=0: means auto guess
87  const double tau = extra_params.getWithDefaultVal("tau", 1e-3);
88  const double e1 = extra_params.getWithDefaultVal("e1",1e-6);
89  const double e2 = extra_params.getWithDefaultVal("e2",1e-6);
90 
91  const double SCALE_HESSIAN = extra_params.getWithDefaultVal("scale_hessian",1);
92 
93 
94  mrpt::utils::CTimeLogger profiler(enable_profiler);
95  profiler.enter("optimize_graph_spa_levmarq (entire)");
96 
97  // Make list of node IDs to optimize, since the user may want only a subset of them to be optimized:
98  profiler.enter("optimize_graph_spa_levmarq.list_IDs"); // ---------------\ .
99  const set<TNodeID> * nodes_to_optimize;
100  set<TNodeID> nodes_to_optimize_auxlist; // Used only if in_nodes_to_optimize==NULL
101  if (in_nodes_to_optimize)
102  {
103  nodes_to_optimize = in_nodes_to_optimize;
104  }
105  else
106  {
107  // We have to make the list of IDs:
108  for (typename gst::graph_t::global_poses_t::const_iterator it=graph.nodes.begin();it!=graph.nodes.end();++it)
109  if (it->first!=graph.root) // Root node is fixed.
110  nodes_to_optimize_auxlist.insert(nodes_to_optimize_auxlist.end(), it->first ); // Provide the "first guess" insert position for efficiency
111  nodes_to_optimize = &nodes_to_optimize_auxlist;
112  }
113  profiler.leave("optimize_graph_spa_levmarq.list_IDs"); // ---------------/
114 
115  // Number of nodes to optimize, or free variables:
116  const size_t nFreeNodes = nodes_to_optimize->size();
117  ASSERT_ABOVE_(nFreeNodes,0)
118 
119  if (verbose)
120  {
121  cout << "["<<__CURRENT_FUNCTION_NAME__<<"] " << nFreeNodes << " nodes to optimize: ";
122  if (nFreeNodes<14)
123  {
124  ostream_iterator<TNodeID> out_it (cout,", ");
125  std::copy(nodes_to_optimize->begin(), nodes_to_optimize->end(), out_it );
126  }
127  cout << endl;
128  }
129 
130  // The list of those edges that will be considered in this optimization (many may be discarded
131  // if we are optimizing just a subset of all the nodes):
132  typedef typename gst::observation_info_t observation_info_t;
133  vector<observation_info_t> lstObservationData;
134 
135  // Note: We'll need those Jacobians{i->j} where at least one "i" or "j"
136  // is a free variable (i.e. it's in nodes_to_optimize)
137  // Now, build the list of all relevent "observations":
138  for (typename gst::edge_const_iterator it=graph.edges.begin();it!=graph.edges.end();++it)
139  {
140  const TPairNodeIDs &ids = it->first;
141  const typename gst::graph_t::edge_t &edge = it->second;
142 
143  if (nodes_to_optimize->find(ids.first)==nodes_to_optimize->end() &&
144  nodes_to_optimize->find(ids.second)==nodes_to_optimize->end())
145  continue; // Skip this edge, none of the IDs are free variables.
146 
147  // get the current global poses of both nodes in this constraint:
148  typename gst::graph_t::global_poses_t::iterator itP1 = graph.nodes.find(ids.first);
149  typename gst::graph_t::global_poses_t::iterator itP2 = graph.nodes.find(ids.second);
150  ASSERTDEBMSG_(itP1!=graph.nodes.end(),"Node1 in an edge does not have a global pose in 'graph.nodes'.")
151  ASSERTDEBMSG_(itP2!=graph.nodes.end(),"Node2 in an edge does not have a global pose in 'graph.nodes'.")
152 
153  const typename gst::graph_t::constraint_t::type_value &EDGE_POSE = edge.getPoseMean();
154 
155  // Add all the data to the list of relevant observations:
156  typename gst::observation_info_t new_entry;
157  new_entry.edge = it;
158  new_entry.edge_mean = &EDGE_POSE;
159  new_entry.P1 = &itP1->second;
160  new_entry.P2 = &itP2->second;
161 
162  lstObservationData.push_back(new_entry);
163  }
164 
165  // The number of constraints, or observations actually implied in this problem:
166  const size_t nObservations = lstObservationData.size();
167  ASSERT_ABOVE_(nObservations,0)
168 
169  // Cholesky object, as a pointer to reuse it between iterations:
170 #if MRPT_HAS_CXX11
171  typedef std::unique_ptr<CSparseMatrix::CholeskyDecomp> SparseCholeskyDecompPtr;
172 #else
173  typedef std::auto_ptr<CSparseMatrix::CholeskyDecomp> SparseCholeskyDecompPtr;
174 #endif
175  SparseCholeskyDecompPtr ptrCh;
176 
177  // The list of Jacobians: for each constraint i->j,
178  // we need the pair of Jacobians: { dh(xi,xj)_dxi, dh(xi,xj)_dxj },
179  // which are "first" and "second" in each pair.
180  // Index of the map are the node IDs {i,j} for each contraint.
181  typename gst::map_pairIDs_pairJacobs_t lstJacobians;
182  // The vector of errors: err_k = SE(2/3)::pseudo_Ln( P_i * EDGE_ij * inv(P_j) )
183  typename mrpt::aligned_containers<typename gst::Array_O>::vector_t errs; // Separated vectors for each edge. i \in [0,nObservations-1], in same order than lstObservationData
184 
185  // ===================================
186  // Compute Jacobians & errors
187  // ===================================
188  profiler.enter("optimize_graph_spa_levmarq.Jacobians&err");// ------------------------------\ .
189  double total_sqr_err = computeJacobiansAndErrors<GRAPH_T>(
190  graph, lstObservationData,
191  lstJacobians, errs);
192  profiler.leave("optimize_graph_spa_levmarq.Jacobians&err"); // ------------------------------/
193 
194 
195  // Only once (since this will be static along iterations), build a quick look-up table with the
196  // indices of the free nodes associated to the (first_id,second_id) of each Jacobian pair:
197  // -----------------------------------------------------------------------------------------------
198  vector<pair<size_t,size_t> > observationIndex_to_relatedFreeNodeIndex; // "relatedFreeNodeIndex" means into [0,nFreeNodes-1], or "-1" if that node is fixed, as ordered in "nodes_to_optimize"
199  observationIndex_to_relatedFreeNodeIndex.reserve(nObservations);
200  ASSERTDEB_(lstJacobians.size()==nObservations)
201  for (typename gst::map_pairIDs_pairJacobs_t::const_iterator itJ=lstJacobians.begin();itJ!=lstJacobians.end();++itJ)
202  {
203  const TNodeID id1 = itJ->first.first;
204  const TNodeID id2 = itJ->first.second;
205  observationIndex_to_relatedFreeNodeIndex.push_back(
206  std::make_pair(
207  mrpt::utils::find_in_vector(id1,*nodes_to_optimize),
208  mrpt::utils::find_in_vector(id2,*nodes_to_optimize) ));
209  }
210 
211  // other important vars for the main loop:
212  CVectorDouble grad(nFreeNodes*DIMS_POSE);
213  grad.setZero();
214  typedef typename mrpt::aligned_containers<TNodeID,typename gst::matrix_VxV_t>::map_t map_ID2matrix_VxV_t;
215  vector<map_ID2matrix_VxV_t> H_map(nFreeNodes);
216 
217  double lambda = initial_lambda; // Will be actually set on first iteration.
218  double v = 1; // was 2, changed since it's modified in the first pass.
219  bool have_to_recompute_H_and_grad = true;
220 
221  // -----------------------------------------------------------
222  // Main iterative loop of Levenberg-Marquardt algorithm
223  // For notation and overall algorithm overview, see:
224  // http://www.mrpt.org/Levenberg%E2%80%93Marquardt_algorithm
225  // -----------------------------------------------------------
226  size_t last_iter = 0;
227 
228  for (size_t iter=0;iter<max_iters;++iter)
229  {
230  last_iter = iter;
231 
232  if (have_to_recompute_H_and_grad) // This will be false only when the delta leads to a worst solution and only a change in lambda is needed.
233  {
234  have_to_recompute_H_and_grad = false;
235 
236  // ========================================================================
237  // Compute the gradient: grad = J^t * errs
238  // ========================================================================
239  // "grad" can be seen as composed of N independent arrays, each one being:
240  // grad_i = \sum_k J^t_{k->i} errs_k
241  // that is: g_i is the "dot-product" of the i'th (transposed) block-column of J and the vector of errors "errs"
242  profiler.enter("optimize_graph_spa_levmarq.grad"); // ------------------------------\ .
243  typename mrpt::aligned_containers<typename gst::Array_O>::vector_t grad_parts(nFreeNodes, array_O_zeros);
244 
245  // "lstJacobians" is sorted in the same order than "lstObservationData":
246  ASSERT_EQUAL_(lstJacobians.size(),lstObservationData.size())
247 
248  {
249  size_t idx_obs;
250  typename gst::map_pairIDs_pairJacobs_t::const_iterator itJ;
251 
252  for (idx_obs=0, itJ=lstJacobians.begin();
253  itJ!=lstJacobians.end();
254  ++itJ,++idx_obs)
255  {
256  ASSERTDEB_(itJ->first==lstObservationData[idx_obs].edge->first) // make sure they're in the expected order!
257 
258  // grad[k] += J^t_{i->k} * Inf.Matrix * errs_i
259  // k: [0,nFreeNodes-1] <-- IDs.first & IDs.second
260  // i: [0,nObservations-1] <--- idx_obs
261 
262  // Get the corresponding indices in the vector of "free variables" being optimized:
263  const size_t idx1 = observationIndex_to_relatedFreeNodeIndex[idx_obs].first;
264  const size_t idx2 = observationIndex_to_relatedFreeNodeIndex[idx_obs].second;
265 
266  if (idx1!=string::npos)
267  detail::AuxErrorEval<typename gst::edge_t,gst>::multiply_Jt_W_err(
268  itJ->second.first /* J */,
269  lstObservationData[idx_obs].edge /* W */,
270  errs[idx_obs] /* err */,
271  grad_parts[idx1] /* out */
272  );
273 
274  if (idx2!=string::npos)
275  detail::AuxErrorEval<typename gst::edge_t,gst>::multiply_Jt_W_err(
276  itJ->second.second /* J */,
277  lstObservationData[idx_obs].edge /* W */,
278  errs[idx_obs] /* err */,
279  grad_parts[idx2] /* out */
280  );
281  }
282  }
283 
284  // build the gradient as a single vector:
285  ::memcpy(&grad[0],&grad_parts[0], nFreeNodes*DIMS_POSE*sizeof(grad[0])); // Ohh yeahh!
286  grad /= SCALE_HESSIAN;
287  profiler.leave("optimize_graph_spa_levmarq.grad"); // ------------------------------/
288 
289  // End condition #1
290  const double grad_norm_inf = math::norm_inf(grad); // inf-norm (abs. maximum value) of the gradient
291  if (grad_norm_inf<=e1)
292  {
293  // Change is too small
294  if (verbose) cout << "["<<__CURRENT_FUNCTION_NAME__<<"] " << mrpt::format("End condition #1: math::norm_inf(g)<=e1 :%f<=%f\n",grad_norm_inf,e1);
295  break;
296  }
297 
298 
299  profiler.enter("optimize_graph_spa_levmarq.sp_H:build map"); // ------------------------------\ .
300  // ======================================================================
301  // Build sparse representation of the upper triangular part of
302  // the Hessian matrix H = J^t * J
303  //
304  // Sparse memory structure is a vector of maps, such as:
305  // - H_map[i]: corresponds to the i'th column of H.
306  // Here "i" corresponds to [0,N-1] indices of appearance in the map "*nodes_to_optimize".
307  // - H_map[i][j] is the entry for the j'th row, with "j" also in the range [0,N-1] as ordered in "*nodes_to_optimize".
308  // ======================================================================
309  {
310  size_t idxObs;
311  typename gst::map_pairIDs_pairJacobs_t::const_iterator itJacobPair;
312 
313  for (idxObs=0, itJacobPair=lstJacobians.begin();
314  idxObs<nObservations;
315  ++itJacobPair,++idxObs)
316  {
317  // We sort IDs such as "i" < "j" and we can build just the upper triangular part of the Hessian.
318  const bool edge_straight = itJacobPair->first.first < itJacobPair->first.second;
319 
320  // Indices in the "H_map" vector:
321  const size_t idx_i = edge_straight ? observationIndex_to_relatedFreeNodeIndex[idxObs].first : observationIndex_to_relatedFreeNodeIndex[idxObs].second;
322  const size_t idx_j = edge_straight ? observationIndex_to_relatedFreeNodeIndex[idxObs].second : observationIndex_to_relatedFreeNodeIndex[idxObs].first;
323 
324  const bool is_i_free_node = idx_i!=string::npos;
325  const bool is_j_free_node = idx_j!=string::npos;
326 
327  // Take references to both Jacobians (wrt pose "i" and pose "j"), taking into account the possible
328  // switch in their order:
329 
330  const typename gst::matrix_VxV_t &J1 = edge_straight ? itJacobPair->second.first : itJacobPair->second.second;
331  const typename gst::matrix_VxV_t &J2 = edge_straight ? itJacobPair->second.second : itJacobPair->second.first;
332 
333  // Is "i" a free (to be optimized) node? -> Ji^t * Inf * Ji
334  if (is_i_free_node)
335  {
336  typename gst::matrix_VxV_t JtJ(mrpt::math::UNINITIALIZED_MATRIX);
337  detail::AuxErrorEval<typename gst::edge_t,gst>::multiplyJtLambdaJ(J1,JtJ,lstObservationData[idxObs].edge);
338  H_map[idx_i][idx_i] += JtJ;
339  }
340  // Is "j" a free (to be optimized) node? -> Jj^t * Inf * Jj
341  if (is_j_free_node)
342  {
343  typename gst::matrix_VxV_t JtJ(mrpt::math::UNINITIALIZED_MATRIX);
344  detail::AuxErrorEval<typename gst::edge_t,gst>::multiplyJtLambdaJ(J2,JtJ,lstObservationData[idxObs].edge);
345  H_map[idx_j][idx_j] += JtJ;
346  }
347  // Are both "i" and "j" free nodes? -> Ji^t * Inf * Jj
348  if (is_i_free_node && is_j_free_node)
349  {
350  typename gst::matrix_VxV_t JtJ(mrpt::math::UNINITIALIZED_MATRIX);
351  detail::AuxErrorEval<typename gst::edge_t,gst>::multiplyJ1tLambdaJ2(J1,J2,JtJ,lstObservationData[idxObs].edge);
352  H_map[idx_j][idx_i] += JtJ;
353  }
354  }
355  }
356  profiler.leave("optimize_graph_spa_levmarq.sp_H:build map"); // ------------------------------/
357 
358  // Just in the first iteration, we need to calculate an estimate for the first value of "lamdba":
359  if (lambda<=0 && iter==0)
360  {
361  profiler.enter("optimize_graph_spa_levmarq.lambda_init"); // ---\ .
362  double H_diagonal_max = 0;
363  for (size_t i=0;i<nFreeNodes;i++)
364  for (typename map_ID2matrix_VxV_t::const_iterator it=H_map[i].begin();it!=H_map[i].end();++it)
365  {
366  const size_t j = it->first; // entry submatrix is for (i,j).
367  if (i==j)
368  {
369  for (size_t k=0;k<DIMS_POSE;k++)
370  mrpt::utils::keep_max(H_diagonal_max, it->second.get_unsafe(k,k) );
371  }
372  }
373  lambda = tau * H_diagonal_max;
374 
375  profiler.leave("optimize_graph_spa_levmarq.lambda_init"); // ---/
376  }
377  else
378  {
379  // After recomputing H and the grad, we update lambda:
380  lambda *= 0.1; //std::max(0.333, 1-pow(2*rho-1,3.0) );
381  }
382  utils::keep_max(lambda, 1e-200); // JL: Avoids underflow!
383  v = 2;
384  #if 0
385  { mrpt::math::CMatrixDouble H; sp_H.get_dense(H); H.saveToTextFile("d:\\H.txt"); }
386  #endif
387  } // end "have_to_recompute_H_and_grad"
388 
389  if (verbose )
390  cout << "["<<__CURRENT_FUNCTION_NAME__<<"] Iter: " << iter << " ,total sqr. err: " << total_sqr_err << ", avrg. err per edge: " << std::sqrt(total_sqr_err/nObservations) << " lambda: " << lambda << endl;
391 
392  // Feedback to the user:
393  if (functor_feedback)
394  {
395  (*functor_feedback)(graph,iter,max_iters,total_sqr_err);
396  }
397 
398 
399  profiler.enter("optimize_graph_spa_levmarq.sp_H:build"); // ------------------------------\ .
400  // Now, build the actual sparse matrix H:
401  // Note: we only need to fill out the upper diagonal part, since Cholesky will later on ignore the other part.
402  CSparseMatrix sp_H(nFreeNodes*DIMS_POSE,nFreeNodes*DIMS_POSE);
403  for (size_t i=0;i<nFreeNodes;i++)
404  {
405  const size_t i_offset = i*DIMS_POSE;
406 
407  for (typename map_ID2matrix_VxV_t::const_iterator it=H_map[i].begin();it!=H_map[i].end();++it)
408  {
409  const size_t j = it->first; // entry submatrix is for (i,j).
410  const size_t j_offset = j*DIMS_POSE;
411 
412  // For i==j (diagonal blocks), it's different, since we only need to insert their
413  // upper-diagonal half and also we have to add the lambda*I to the diagonal from the Lev-Marq. algorithm:
414  if (i==j)
415  {
416  for (size_t r=0;r<DIMS_POSE;r++)
417  {
418  // c=r: add lambda from LM
419  sp_H.insert_entry_fast(j_offset+r,i_offset+r, it->second.get_unsafe(r,r) + lambda );
420  // c>r:
421  for (size_t c=r+1;c<DIMS_POSE;c++)
422  sp_H.insert_entry_fast(j_offset+r,i_offset+c, it->second.get_unsafe(r,c));
423  }
424  }
425  else
426  {
427  sp_H.insert_submatrix(j_offset,i_offset, it->second);
428  }
429  }
430  } // end for each free node, build sp_H
431 
432  sp_H.compressFromTriplet();
433  profiler.leave("optimize_graph_spa_levmarq.sp_H:build"); // ------------------------------/
434 
435  // Use the cparse Cholesky decomposition to efficiently solve:
436  // (H+\lambda*I) \delta = -J^t * (f(x)-z)
437  // A x = b --> x = A^{-1} * b
438  //
439  CVectorDouble delta; // The (minus) increment to be added to the current solution in this step
440  try
441  {
442  profiler.enter("optimize_graph_spa_levmarq.sp_H:chol");
443  if (!ptrCh.get())
444  ptrCh = SparseCholeskyDecompPtr(new CSparseMatrix::CholeskyDecomp(sp_H) );
445  else ptrCh.get()->update(sp_H);
446  profiler.leave("optimize_graph_spa_levmarq.sp_H:chol");
447 
448  profiler.enter("optimize_graph_spa_levmarq.sp_H:backsub");
449  ptrCh->backsub(grad,delta);
450  profiler.leave("optimize_graph_spa_levmarq.sp_H:backsub");
451  }
452  catch (CExceptionNotDefPos &)
453  {
454  // not positive definite so increase mu and try again
455  if (verbose ) cout << "["<<__CURRENT_FUNCTION_NAME__<<"] Got non-definite positive matrix, retrying with a larger lambda...\n";
456  lambda *= v;
457  v*= 2;
458  if (lambda>1e9)
459  { // enough!
460  break;
461  }
462  continue; // try again with this params
463  }
464 
465  // Compute norm of the increment vector:
466  profiler.enter("optimize_graph_spa_levmarq.delta_norm");
467  const double delta_norm = math::norm(delta);
468  profiler.leave("optimize_graph_spa_levmarq.delta_norm");
469 
470  // Compute norm of the current solution vector:
471  profiler.enter("optimize_graph_spa_levmarq.x_norm" );
472  double x_norm = 0;
473  {
474  for (set<TNodeID>::const_iterator it=nodes_to_optimize->begin();it!=nodes_to_optimize->end();++it)
475  {
476  typename gst::graph_t::global_poses_t::const_iterator itP = graph.nodes.find(*it);
477  const typename gst::graph_t::constraint_t::type_value &P = itP->second;
478  for (size_t i=0;i<DIMS_POSE;i++)
479  x_norm+=square(P[i]);
480  }
481  x_norm=std::sqrt(x_norm);
482  }
483  profiler.leave("optimize_graph_spa_levmarq.x_norm" );
484 
485  // Test end condition #2:
486  const double thres_norm = e2*(x_norm+e2);
487  if (delta_norm<thres_norm)
488  {
489  // The change is too small: we're done here...
490  if (verbose ) cout << "["<<__CURRENT_FUNCTION_NAME__<<"] " << format("End condition #2: %e < %e\n", delta_norm, thres_norm );
491  break;
492  }
493  else
494  {
495  // =====================================================================================
496  // Accept this delta? Try it and look at the increase/decrease of the error:
497  // new_x = old_x [+] (-delta) , with [+] being the "manifold exp()+add" operation.
498  // =====================================================================================
499  typename gst::graph_t::global_poses_t old_poses_backup;
500 
501  {
502  ASSERTDEB_(delta.size()==nodes_to_optimize->size()*DIMS_POSE)
503  const double *delta_ptr = &delta[0];
504  for (set<TNodeID>::const_iterator it=nodes_to_optimize->begin();it!=nodes_to_optimize->end();++it)
505  {
506  // exp_delta_i = Exp_SE( delta_i )
507  typename gst::graph_t::constraint_t::type_value exp_delta_pose(UNINITIALIZED_POSE);
508  typename gst::Array_O exp_delta;
509  for (size_t i=0;i<DIMS_POSE;i++)
510  exp_delta[i]= - *delta_ptr++; // The "-" sign is for the missing "-" carried all this time from above
511  gst::SE_TYPE::exp(exp_delta,exp_delta_pose);
512 
513  // new_x_i = exp_delta_i (+) old_x_i
514  typename gst::graph_t::global_poses_t::iterator it_old_value = graph.nodes.find(*it);
515  old_poses_backup[*it] = it_old_value->second; // back up the old pose as a copy
516  it_old_value->second.composeFrom(exp_delta_pose, it_old_value->second);
517  }
518  }
519 
520  // =============================================================
521  // Compute Jacobians & errors with the new "graph.nodes" info:
522  // =============================================================
523  typename gst::map_pairIDs_pairJacobs_t new_lstJacobians;
524  typename mrpt::aligned_containers<typename gst::Array_O>::vector_t new_errs;
525 
526  profiler.enter("optimize_graph_spa_levmarq.Jacobians&err");// ------------------------------\ .
527  double new_total_sqr_err = computeJacobiansAndErrors<GRAPH_T>(
528  graph, lstObservationData,
529  new_lstJacobians, new_errs);
530  profiler.leave("optimize_graph_spa_levmarq.Jacobians&err");// ------------------------------/
531 
532  // Now, to decide whether to accept the change:
533  if (new_total_sqr_err < total_sqr_err) // rho>0)
534  {
535  // Accept the new point:
536  new_lstJacobians.swap(lstJacobians);
537  new_errs.swap(errs);
538  std::swap( new_total_sqr_err, total_sqr_err);
539 
540  // Instruct to recompute H and grad from the new Jacobians.
541  have_to_recompute_H_and_grad = true;
542  }
543  else
544  {
545  // Nope...
546  // We have to revert the "graph.nodes" to "old_poses_backup"
547  for (typename gst::graph_t::global_poses_t::const_iterator it=old_poses_backup.begin();it!=old_poses_backup.end();++it)
548  graph.nodes[it->first] = it->second;
549 
550  if (verbose ) cout << "["<<__CURRENT_FUNCTION_NAME__<<"] Got larger error=" << new_total_sqr_err << ", retrying with a larger lambda...\n";
551  // Change params and try again:
552  lambda *= v;
553  v*= 2;
554  }
555 
556  } // end else end condition #2
557 
558  } // end for each iter
559 
560  profiler.leave("optimize_graph_spa_levmarq (entire)");
561 
562 
563  // Fill out basic output data:
564  // ------------------------------
565  out_info.num_iters = last_iter;
566  out_info.final_total_sq_error = total_sqr_err;
567 
568  MRPT_END
569  } // end of optimize_graph_spa_levmarq()
570 
571 
572  /** @} */ // end of grouping
573 
574  } // End of namespace
575 } // End of namespace
576 
577 #endif
ASSERT_EQUAL_
#define ASSERT_EQUAL_(__A, __B)
Definition: maps/CColouredPointsMap.h:281
mrpt::graphslam::optimize_graph_spa_levmarq
void optimize_graph_spa_levmarq(GRAPH_T &graph, TResultInfoSpaLevMarq &out_info, const std::set< mrpt::utils::TNodeID > *in_nodes_to_optimize=NULL, const mrpt::utils::TParametersDouble &extra_params=mrpt::utils::TParametersDouble(), typename graphslam_traits< GRAPH_T >::TFunctorFeedback functor_feedback=NULL)
Optimize a graph of pose constraints using the Sparse Pose Adjustment (SPA) sparse representation and...
Definition: levmarq.h:56
mrpt::system::os::memcpy
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
mrpt::utils::TPairNodeIDs
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Definition: types_simple.h:46
ids
GLuint * ids
Definition: glext.h:3767
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
ASSERT_ABOVE_
#define ASSERT_ABOVE_(__A, __B)
Definition: maps/CColouredPointsMap.h:284
mrpt::math::CSparseMatrix::insert_entry_fast
void insert_entry_fast(const size_t row, const size_t col, const double val)
This was an optimized version, but is now equivalent to insert_entry() due to the need to be compatib...
Definition: CSparseMatrix.h:254
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
begin
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::graphslam::TResultInfoSpaLevMarq::num_iters
size_t num_iters
The number of LM iterations executed.
Definition: graphslam/include/mrpt/graphslam/types.h:60
mrpt::aligned_containers::map_t
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
Definition: aligned_containers.h:31
mrpt::math::CSparseMatrix::CholeskyDecomp
Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
Definition: CSparseMatrix.h:360
mrpt::math::CSparseMatrix
A sparse matrix structure, wrapping T.
Definition: CSparseMatrix.h:92
std
STL namespace.
mrpt::math::CSparseMatrix::compressFromTriplet
void compressFromTriplet()
ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
Definition: CSparseMatrix.cpp:249
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::math::CExceptionNotDefPos
Used in mrpt::math::CSparseMatrix.
Definition: CSparseMatrix.h:38
ASSERTDEBMSG_
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: maps/CColouredPointsMap.h:301
mrpt::graphslam
SLAM methods related to graphs of pose constraints.
Definition: TUserOptionsChecker.h:32
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:370
c
const GLubyte * c
Definition: glext.h:5590
mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
stl_containers_utils.h
mrpt::utils::TNodeID
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
mrpt::mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
mrpt::utils::keep_max
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Definition: bits.h:176
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::graphslam::graphslam_traits
Auxiliary traits template for use among graph-slam problems to make life easier with these complicate...
Definition: graphslam/include/mrpt/graphslam/types.h:29
mrpt::math::norm_inf
CONTAINER::Scalar norm_inf(const CONTAINER &v)
Definition: ops_containers.h:109
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:366
v
const GLdouble * v
Definition: glext.h:3603
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
TParameters.h
ASSERTDEB_
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: maps/CColouredPointsMap.h:300
r
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
mrpt::math::CSparseMatrix::insert_submatrix
void insert_submatrix(const size_t row, const size_t col, const MATRIX &M)
ONLY for TRIPLET matrices: insert a given matrix (in any of the MRPT formats) at a given location of ...
Definition: CSparseMatrix.h:263
mrpt::graphslam::TResultInfoSpaLevMarq
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
Definition: graphslam/include/mrpt/graphslam/types.h:58
mrpt::utils::CTimeLogger
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X), among other stats.
Definition: CTimeLogger.h:41
mrpt::utils::CTimeLogger::leave
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
__CURRENT_FUNCTION_NAME__
#define __CURRENT_FUNCTION_NAME__
A macro for obtaining the name of the current function:
Definition: maps/CColouredPointsMap.h:146
mrpt::utils::find_in_vector
size_t find_in_vector(const T &value, const CONTAINER &vect)
Returns the index of the value "T" in the container "vect" (std::vector,std::deque,etc), or string::npos if not found.
Definition: stl_containers_utils.h:27
mrpt::utils::CTimeLogger::enter
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
levmarq_impl.h
mrpt::mrpt::utils::keep_max
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Definition: maps/CColouredPointsMap.h:176
mrpt::graphslam::TResultInfoSpaLevMarq::final_total_sq_error
double final_total_sq_error
The sum of all the squared errors for every constraint involved in the problem.
Definition: graphslam/include/mrpt/graphslam/types.h:61
mrpt::aligned_containers::vector_t
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
Definition: aligned_containers.h:28
mrpt::math::norm
CONTAINER::Scalar norm(const CONTAINER &v)
Definition: ops_containers.h:110


Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019