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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019