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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020