Main MRPT website > C++ reference for MRPT 1.5.7
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  // "relatedFreeNodeIndex" means into [0,nFreeNodes-1], or "-1" if that node is fixed, as ordered in "nodes_to_optimize"
199  vector<pair<size_t,size_t> > obsIdx2fnIdx;
200  obsIdx2fnIdx.reserve(nObservations);
201  ASSERTDEB_(lstJacobians.size()==nObservations)
202  for (typename gst::map_pairIDs_pairJacobs_t::const_iterator itJ=lstJacobians.begin();itJ!=lstJacobians.end();++itJ)
203  {
204  const TNodeID id1 = itJ->first.first;
205  const TNodeID id2 = itJ->first.second;
206  obsIdx2fnIdx.push_back(
207  std::make_pair(
208  mrpt::utils::find_in_vector(id1,*nodes_to_optimize),
209  mrpt::utils::find_in_vector(id2,*nodes_to_optimize) ));
210  }
211 
212  // other important vars for the main loop:
213  CVectorDouble grad(nFreeNodes*DIMS_POSE);
214  grad.setZero();
215  typedef typename mrpt::aligned_containers<TNodeID,typename gst::matrix_VxV_t>::map_t map_ID2matrix_VxV_t;
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  vector<map_ID2matrix_VxV_t> H_map(nFreeNodes);
232 
233  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.
234  {
235  have_to_recompute_H_and_grad = false;
236 
237  // ========================================================================
238  // Compute the gradient: grad = J^t * errs
239  // ========================================================================
240  // "grad" can be seen as composed of N independent arrays, each one being:
241  // grad_i = \sum_k J^t_{k->i} errs_k
242  // that is: g_i is the "dot-product" of the i'th (transposed) block-column of J and the vector of errors "errs"
243  profiler.enter("optimize_graph_spa_levmarq.grad"); // ------------------------------\ .
244  typename mrpt::aligned_containers<typename gst::Array_O>::vector_t grad_parts(nFreeNodes, array_O_zeros);
245 
246  // "lstJacobians" is sorted in the same order than "lstObservationData":
247  ASSERT_EQUAL_(lstJacobians.size(),lstObservationData.size())
248 
249  {
250  size_t idx_obs;
251  typename gst::map_pairIDs_pairJacobs_t::const_iterator itJ;
252 
253  for (idx_obs=0, itJ=lstJacobians.begin();
254  itJ!=lstJacobians.end();
255  ++itJ,++idx_obs)
256  {
257  ASSERTDEB_(itJ->first==lstObservationData[idx_obs].edge->first) // make sure they're in the expected order!
258 
259  // grad[k] += J^t_{i->k} * Inf.Matrix * errs_i
260  // k: [0,nFreeNodes-1] <-- IDs.first & IDs.second
261  // i: [0,nObservations-1] <--- idx_obs
262 
263  // Get the corresponding indices in the vector of "free variables" being optimized:
264  const size_t idx1 = obsIdx2fnIdx[idx_obs].first;
265  const size_t idx2 = obsIdx2fnIdx[idx_obs].second;
266 
267  if (idx1!=string::npos)
268  detail::AuxErrorEval<typename gst::edge_t,gst>::multiply_Jt_W_err(
269  itJ->second.first /* J */,
270  lstObservationData[idx_obs].edge /* W */,
271  errs[idx_obs] /* err */,
272  grad_parts[idx1] /* out */
273  );
274 
275  if (idx2!=string::npos)
276  detail::AuxErrorEval<typename gst::edge_t,gst>::multiply_Jt_W_err(
277  itJ->second.second /* J */,
278  lstObservationData[idx_obs].edge /* W */,
279  errs[idx_obs] /* err */,
280  grad_parts[idx2] /* out */
281  );
282  }
283  }
284 
285  // build the gradient as a single vector:
286  ::memcpy(&grad[0],&grad_parts[0], nFreeNodes*DIMS_POSE*sizeof(grad[0])); // Ohh yeahh!
287  grad /= SCALE_HESSIAN;
288  profiler.leave("optimize_graph_spa_levmarq.grad"); // ------------------------------/
289 
290  // End condition #1
291  const double grad_norm_inf = math::norm_inf(grad); // inf-norm (abs. maximum value) of the gradient
292  if (grad_norm_inf<=e1)
293  {
294  // Change is too small
295  if (verbose) cout << "["<<__CURRENT_FUNCTION_NAME__<<"] " << mrpt::format("End condition #1: math::norm_inf(g)<=e1 :%f<=%f\n",grad_norm_inf,e1);
296  break;
297  }
298 
299 
300  profiler.enter("optimize_graph_spa_levmarq.sp_H:build map"); // ------------------------------\ .
301  // ======================================================================
302  // Build sparse representation of the upper triangular part of
303  // the Hessian matrix H = J^t * J
304  //
305  // Sparse memory structure is a vector of maps, such as:
306  // - H_map[i]: corresponds to the i'th column of H.
307  // Here "i" corresponds to [0,N-1] indices of appearance in the map "*nodes_to_optimize".
308  // - 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".
309  // ======================================================================
310  {
311  size_t idxObs;
312  typename gst::map_pairIDs_pairJacobs_t::const_iterator itJacobPair;
313 
314  for (idxObs=0, itJacobPair=lstJacobians.begin();
315  idxObs<nObservations;
316  ++itJacobPair,++idxObs)
317  {
318  const bool Hij_upper_triang =
319  itJacobPair->first.first < itJacobPair->first.second;
320 
321  // Indices in the "H_map" vector:
322  const size_t idx_i = obsIdx2fnIdx[idxObs].first;
323  const size_t idx_j = obsIdx2fnIdx[idxObs].second;
324 
325  const bool is_i_free_node = idx_i!=string::npos;
326  const bool is_j_free_node = idx_j!=string::npos;
327 
328  // Take references to both Jacobians (wrt pose "i" and pose "j"), taking into account the possible
329  // switch in their order:
330  const typename gst::matrix_VxV_t& J1 = itJacobPair->second.first;
331  const typename gst::matrix_VxV_t& J2 = itJacobPair->second.second;
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  // We sort IDs such as "i" < "j" and we can build just
353  // the upper triangular part of the Hessian:
354  if (Hij_upper_triang) // H_map[col][row]
355  H_map[idx_j][idx_i] += JtJ;
356  else
357  H_map[idx_i][idx_j] += JtJ.transpose();
358  }
359  }
360  }
361  profiler.leave("optimize_graph_spa_levmarq.sp_H:build map"); // ------------------------------/
362 
363  // Just in the first iteration, we need to calculate an estimate for the first value of "lamdba":
364  if (lambda<=0 && iter==0)
365  {
366  profiler.enter("optimize_graph_spa_levmarq.lambda_init"); // ---\ .
367  double H_diagonal_max = 0;
368  for (size_t i=0;i<nFreeNodes;i++)
369  for (typename map_ID2matrix_VxV_t::const_iterator it=H_map[i].begin();it!=H_map[i].end();++it)
370  {
371  const size_t j = it->first; // entry submatrix is for (i,j).
372  if (i==j)
373  {
374  for (size_t k=0;k<DIMS_POSE;k++)
375  mrpt::utils::keep_max(H_diagonal_max, it->second.get_unsafe(k,k) );
376  }
377  }
378  lambda = tau * H_diagonal_max;
379 
380  profiler.leave("optimize_graph_spa_levmarq.lambda_init"); // ---/
381  }
382  else
383  {
384  // After recomputing H and the grad, we update lambda:
385  lambda *= 0.1; //std::max(0.333, 1-pow(2*rho-1,3.0) );
386  }
387  utils::keep_max(lambda, 1e-200); // JL: Avoids underflow!
388  v = 2;
389  #if 0
390  { mrpt::math::CMatrixDouble H; sp_H.get_dense(H); H.saveToTextFile("d:\\H.txt"); }
391  #endif
392  } // end "have_to_recompute_H_and_grad"
393 
394  if (verbose )
395  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;
396 
397  // Feedback to the user:
398  if (functor_feedback)
399  {
400  (*functor_feedback)(graph,iter,max_iters,total_sqr_err);
401  }
402 
403 
404  profiler.enter("optimize_graph_spa_levmarq.sp_H:build"); // ------------------------------\ .
405  // Now, build the actual sparse matrix H:
406  // Note: we only need to fill out the upper diagonal part, since Cholesky will later on ignore the other part.
407  CSparseMatrix sp_H(nFreeNodes*DIMS_POSE,nFreeNodes*DIMS_POSE);
408  for (size_t i=0;i<nFreeNodes;i++)
409  {
410  const size_t i_offset = i*DIMS_POSE;
411 
412  for (typename map_ID2matrix_VxV_t::const_iterator it=H_map[i].begin();it!=H_map[i].end();++it)
413  {
414  const size_t j = it->first; // entry submatrix is for (i,j).
415  const size_t j_offset = j*DIMS_POSE;
416 
417  // For i==j (diagonal blocks), it's different, since we only need to insert their
418  // upper-diagonal half and also we have to add the lambda*I to the diagonal from the Lev-Marq. algorithm:
419  if (i==j)
420  {
421  for (size_t r=0;r<DIMS_POSE;r++)
422  {
423  // c=r: add lambda from LM
424  sp_H.insert_entry_fast(j_offset+r,i_offset+r, it->second.get_unsafe(r,r) + lambda );
425  // c>r:
426  for (size_t c=r+1;c<DIMS_POSE;c++)
427  sp_H.insert_entry_fast(j_offset+r,i_offset+c, it->second.get_unsafe(r,c));
428  }
429  }
430  else
431  {
432  sp_H.insert_submatrix(j_offset,i_offset, it->second);
433  }
434  }
435  } // end for each free node, build sp_H
436 
437  sp_H.compressFromTriplet();
438  profiler.leave("optimize_graph_spa_levmarq.sp_H:build"); // ------------------------------/
439 
440  // Use the cparse Cholesky decomposition to efficiently solve:
441  // (H+\lambda*I) \delta = -J^t * (f(x)-z)
442  // A x = b --> x = A^{-1} * b
443  //
444  CVectorDouble delta; // The (minus) increment to be added to the current solution in this step
445  try
446  {
447  profiler.enter("optimize_graph_spa_levmarq.sp_H:chol");
448  if (!ptrCh.get())
449  ptrCh = SparseCholeskyDecompPtr(new CSparseMatrix::CholeskyDecomp(sp_H) );
450  else ptrCh.get()->update(sp_H);
451  profiler.leave("optimize_graph_spa_levmarq.sp_H:chol");
452 
453  profiler.enter("optimize_graph_spa_levmarq.sp_H:backsub");
454  ptrCh->backsub(grad,delta);
455  profiler.leave("optimize_graph_spa_levmarq.sp_H:backsub");
456  }
457  catch (CExceptionNotDefPos &)
458  {
459  // not positive definite so increase mu and try again
460  if (verbose ) cout << "["<<__CURRENT_FUNCTION_NAME__<<"] Got non-definite positive matrix, retrying with a larger lambda...\n";
461  lambda *= v;
462  v*= 2;
463  if (lambda>1e9)
464  { // enough!
465  break;
466  }
467  continue; // try again with this params
468  }
469 
470  // Compute norm of the increment vector:
471  profiler.enter("optimize_graph_spa_levmarq.delta_norm");
472  const double delta_norm = math::norm(delta);
473  profiler.leave("optimize_graph_spa_levmarq.delta_norm");
474 
475  // Compute norm of the current solution vector:
476  profiler.enter("optimize_graph_spa_levmarq.x_norm" );
477  double x_norm = 0;
478  {
479  for (set<TNodeID>::const_iterator it=nodes_to_optimize->begin();it!=nodes_to_optimize->end();++it)
480  {
481  typename gst::graph_t::global_poses_t::const_iterator itP = graph.nodes.find(*it);
482  const typename gst::graph_t::constraint_t::type_value &P = itP->second;
483  for (size_t i=0;i<DIMS_POSE;i++)
484  x_norm+=square(P[i]);
485  }
486  x_norm=std::sqrt(x_norm);
487  }
488  profiler.leave("optimize_graph_spa_levmarq.x_norm" );
489 
490  // Test end condition #2:
491  const double thres_norm = e2*(x_norm+e2);
492  if (delta_norm<thres_norm)
493  {
494  // The change is too small: we're done here...
495  if (verbose ) cout << "["<<__CURRENT_FUNCTION_NAME__<<"] " << format("End condition #2: %e < %e\n", delta_norm, thres_norm );
496  break;
497  }
498  else
499  {
500  // =====================================================================================
501  // Accept this delta? Try it and look at the increase/decrease of the error:
502  // new_x = old_x [+] (-delta) , with [+] being the "manifold exp()+add" operation.
503  // =====================================================================================
504  typename gst::graph_t::global_poses_t old_poses_backup;
505 
506  {
507  ASSERTDEB_(delta.size()==nodes_to_optimize->size()*DIMS_POSE)
508  const double *delta_ptr = &delta[0];
509  for (set<TNodeID>::const_iterator it=nodes_to_optimize->begin();it!=nodes_to_optimize->end();++it)
510  {
511  // exp_delta_i = Exp_SE( delta_i )
512  typename gst::Array_O exp_delta;
513  for (size_t i=0;i<DIMS_POSE;i++)
514  exp_delta[i]= - *delta_ptr++; // The "-" sign is for the missing "-" carried all this time from above
515 
516  // new_x_i = exp_delta_i (+) old_x_i
517  typename gst::graph_t::global_poses_t::iterator it_old_value = graph.nodes.find(*it);
518  old_poses_backup[*it] = it_old_value->second; // back up the old pose as a copy
519  detail::AuxPoseOPlus<typename gst::edge_t, gst>::sumIncr(it_old_value->second, exp_delta);
520  }
521  }
522 
523  // =============================================================
524  // Compute Jacobians & errors with the new "graph.nodes" info:
525  // =============================================================
526  typename gst::map_pairIDs_pairJacobs_t new_lstJacobians;
527  typename mrpt::aligned_containers<typename gst::Array_O>::vector_t new_errs;
528 
529  profiler.enter("optimize_graph_spa_levmarq.Jacobians&err");// ------------------------------\ .
530  double new_total_sqr_err = computeJacobiansAndErrors<GRAPH_T>(
531  graph, lstObservationData,
532  new_lstJacobians, new_errs);
533  profiler.leave("optimize_graph_spa_levmarq.Jacobians&err");// ------------------------------/
534 
535  // Now, to decide whether to accept the change:
536  if (new_total_sqr_err < total_sqr_err) // rho>0)
537  {
538  // Accept the new point:
539  new_lstJacobians.swap(lstJacobians);
540  new_errs.swap(errs);
541  std::swap( new_total_sqr_err, total_sqr_err);
542 
543  // Instruct to recompute H and grad from the new Jacobians.
544  have_to_recompute_H_and_grad = true;
545  }
546  else
547  {
548  // Nope...
549  // We have to revert the "graph.nodes" to "old_poses_backup"
550  for (typename gst::graph_t::global_poses_t::const_iterator it=old_poses_backup.begin();it!=old_poses_backup.end();++it)
551  graph.nodes[it->first] = it->second;
552 
553  if (verbose ) cout << "["<<__CURRENT_FUNCTION_NAME__<<"] Got larger error=" << new_total_sqr_err << ", retrying with a larger lambda...\n";
554  // Change params and try again:
555  lambda *= v;
556  v*= 2;
557  }
558 
559  } // end else end condition #2
560 
561  } // end for each iter
562 
563  profiler.leave("optimize_graph_spa_levmarq (entire)");
564 
565 
566  // Fill out basic output data:
567  // ------------------------------
568  out_info.num_iters = last_iter;
569  out_info.final_total_sq_error = total_sqr_err;
570 
571  MRPT_END
572  } // end of optimize_graph_spa_levmarq()
573 
574 
575  /** @} */ // end of grouping
576 
577  } // End of namespace
578 } // End of namespace
579 
580 #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.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019