Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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
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
19namespace 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>
57 GRAPH_T & graph,
58 TResultInfoSpaLevMarq & out_info,
59 const std::set<mrpt::utils::TNodeID> * in_nodes_to_optimize = NULL,
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
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();
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)
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)
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;
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
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
Used in mrpt::math::CSparseMatrix.
Definition: CSparseMatrix.h:39
Auxiliary class to hold the results of a Cholesky factorization of a sparse matrix.
A sparse matrix structure, wrapping T.
Definition: CSparseMatrix.h:93
void compressFromTriplet()
ONLY for TRIPLET matrices: convert the matrix in a column-compressed form.
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 ...
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...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: types_math.h:65
A versatile "profiler" that logs the time spent within each pair of calls to enter(X)-leave(X),...
Definition: CTimeLogger.h:36
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:77
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:82
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
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
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
std::pair< TNodeID, TNodeID > TPairNodeIDs
A pair of node IDs.
Definition: types_simple.h:46
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,...
#define ASSERT_EQUAL_(__A, __B)
Definition: mrpt_macros.h:264
#define ASSERT_ABOVE_(__A, __B)
Definition: mrpt_macros.h:267
#define MRPT_START
Definition: mrpt_macros.h:349
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: mrpt_macros.h:283
#define MRPT_END
Definition: mrpt_macros.h:353
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: mrpt_macros.h:284
#define __CURRENT_FUNCTION_NAME__
A macro for obtaining the name of the current function:
Definition: mrpt_macros.h:102
SLAM methods related to graphs of pose constraints.
Definition: levmarq.h:22
This base provides a set of functions for maths stuff.
Definition: CArray.h:19
@ UNINITIALIZED_MATRIX
Definition: math_frwds.h:75
CONTAINER::Scalar norm_inf(const CONTAINER &v)
CONTAINER::Scalar norm(const CONTAINER &v)
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:18
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values,...
Definition: zip.h:16
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:145
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
STL namespace.
std::map< TYPE1, TYPE2, std::less< TYPE1 >, Eigen::aligned_allocator< std::pair< const TYPE1, TYPE2 > > > map_t
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
Output information for mrpt::graphslam::optimize_graph_spa_levmarq()
double final_total_sq_error
The sum of all the squared errors for every constraint involved in the problem.
size_t num_iters
The number of LM iterations executed.
Auxiliary traits template for use among graph-slam problems to make life easier with these complicate...
For usage when passing a dynamic number of (numeric) arguments to a function, by name.
Definition: TParameters.h:47



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 06:08:57 UTC 2023