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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013