Main MRPT website > C++ reference for MRPT 1.9.9
CLoopCloserERD_impl.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #ifndef CLOOPCLOSERERD_IMPL_H
11 #define CLOOPCLOSERERD_IMPL_H
12 #include <mrpt/math/utils.h>
13 #include <mrpt/containers/stl_containers_utils.h>
14 #include <mrpt/opengl/CEllipsoid.h>
15 #include <mrpt/opengl/CSphere.h>
16 #include <mrpt/math/data_utils.h>
17 
18 namespace mrpt
19 {
20 namespace graphslam
21 {
22 namespace deciders
23 {
24 // Ctors, Dtors
25 // //////////////////////////////////
26 template <class GRAPH_T>
27 CLoopCloserERD<GRAPH_T>::CLoopCloserERD()
28  : m_visualize_curr_node_covariance(false),
29  m_curr_node_covariance_color(160, 160, 160, /*alpha = */ 255),
30  m_partitions_full_update(false),
31  m_is_first_time_node_reg(true),
32  m_dijkstra_node_count_thresh(3)
33 {
34  this->initializeLoggers("CLoopCloserERD");
35  m_edge_types_to_nums["ICP2D"] = 0;
36  m_edge_types_to_nums["LC"] = 0;
37  MRPT_LOG_DEBUG_STREAM("Initialized class object");
38 }
39 
40 template <class GRAPH_T>
41 CLoopCloserERD<GRAPH_T>::~CLoopCloserERD()
42 {
43  using namespace mrpt::graphslam;
44 
45  // release memory of m_node_optimal_paths map.
46  MRPT_LOG_DEBUG_STREAM("Releasing memory of m_node_optimal_paths map...");
47  for (auto it = m_node_optimal_paths.begin();
48  it != m_node_optimal_paths.end(); ++it)
49  {
50  delete it->second;
51  }
52 }
53 
54 // Methods implementations
55 // //////////////////////////////////
56 
57 template <class GRAPH_T>
58 bool CLoopCloserERD<GRAPH_T>::updateState(
59  mrpt::obs::CActionCollection::Ptr action,
60  mrpt::obs::CSensoryFrame::Ptr observations,
61  mrpt::obs::CObservation::Ptr observation)
62 {
63  MRPT_START;
64  MRPT_UNUSED_PARAM(action);
65  this->m_time_logger.enter("updateState");
66  using namespace std;
67  using namespace mrpt;
68  using namespace mrpt::obs;
69  using namespace mrpt::obs::utils;
70  using namespace mrpt::opengl;
71  using namespace mrpt::poses;
72  using namespace mrpt::math;
73 
74  // Track the last recorded laser scan
75  {
76  CObservation2DRangeScan::Ptr scan =
77  getObservation<CObservation2DRangeScan>(observations, observation);
78  if (scan)
79  {
80  m_last_laser_scan2D = scan;
81  }
82  }
83 
84  if (!m_first_laser_scan)
85  {
86  m_first_laser_scan = m_last_laser_scan2D;
87  }
88 
89  // check possible prior node registration
90  size_t num_registered =
91  absDiff(this->m_last_total_num_nodes, this->m_graph->nodeCount());
92  bool registered_new_node = num_registered > 0;
93 
94  if (registered_new_node)
95  {
96  MRPT_LOG_DEBUG_STREAM("New node has been registered in the graph!");
97  registered_new_node = true;
98 
99  // either single node registration, or double node registration for the
100  // first time only, unless we override this check.
101  if (!this->m_override_registered_nodes_check)
102  {
103  if (!((num_registered == 1) ^
104  (num_registered == 2 && m_is_first_time_node_reg)))
105  {
106  MRPT_LOG_ERROR_STREAM(
107  "Invalid number of registered nodes since last call to "
108  "updateStates| "
109  "Found \""
110  << num_registered << "\" new nodes.");
111  THROW_EXCEPTION("Invalid number of registered nodes.");
112  }
113  }
114 
115  // first time call:
116  // NRD should have registered *2* nodes; one for the root node and one
117  // for
118  // the first added constraint. Add the first measurement taken to the
119  // root
120  // and the second as usual
121  if (m_is_first_time_node_reg)
122  {
123  MRPT_LOG_WARN_STREAM(
124  "Assigning first laserScan to the graph root node.");
125  this->m_nodes_to_laser_scans2D[this->m_graph->root] =
126  m_first_laser_scan;
127  m_is_first_time_node_reg = false;
128  }
129 
130  // register the new node-laserScan pair
131  this->m_nodes_to_laser_scans2D[this->m_graph->nodeCount() - 1] =
132  m_last_laser_scan2D;
133 
134  if (m_laser_params.use_scan_matching)
135  {
136  // scan match with previous X nodes
137  this->addScanMatchingEdges(this->m_graph->nodeCount() - 1);
138  }
139 
140  // update partitioning scheme with the latest pose/measurement
141  m_partitions_full_update =
142  ((this->m_graph->nodeCount() %
143  m_lc_params.full_partition_per_nodes) == 0 ||
144  this->m_just_inserted_lc)
145  ? true
146  : false;
147  this->updateMapPartitions(
148  m_partitions_full_update,
149  /* is_first_time_node_reg = */ num_registered == 2);
150 
151  // check for loop closures
152  partitions_t partitions_for_LC;
153  this->checkPartitionsForLC(&partitions_for_LC);
154  this->evaluatePartitionsForLC(partitions_for_LC);
155 
156  if (m_visualize_curr_node_covariance)
157  {
158  this->execDijkstraProjection();
159  }
160 
161  this->m_last_total_num_nodes = this->m_graph->nodeCount();
162  }
163 
164  this->m_time_logger.leave("updateState");
165  return true;
166  MRPT_END;
167 }
168 
169 template <class GRAPH_T>
170 void CLoopCloserERD<GRAPH_T>::fetchNodeIDsForScanMatching(
171  const mrpt::graphs::TNodeID& curr_nodeID,
172  std::set<mrpt::graphs::TNodeID>* nodes_set)
173 {
174  ASSERTDEB_(nodes_set);
175 
176  // deal with the case that less than `prev_nodes_for_ICP` nodes have been
177  // registered
178  int fetched_nodeIDs = 0;
179  for (int nodeID_i = static_cast<int>(curr_nodeID) - 1;
180  ((fetched_nodeIDs <= this->m_laser_params.prev_nodes_for_ICP) &&
181  (nodeID_i >= 0)); // <----- I *have* to use int (instead of unsigned)
182  // if I use this condition
183  --nodeID_i)
184  {
185  nodes_set->insert(nodeID_i);
186  fetched_nodeIDs++;
187  }
188 } // end of fetchNodeIDsForScanMatching
189 
190 template <class GRAPH_T>
191 void CLoopCloserERD<GRAPH_T>::addScanMatchingEdges(
192  const mrpt::graphs::TNodeID& curr_nodeID)
193 {
194  MRPT_START;
195  using namespace std;
196  using namespace mrpt;
197  using namespace mrpt::obs;
198  using namespace mrpt::math;
199  using mrpt::graphs::TNodeID;
200 
201  // get a list of nodes to check ICP against
202  MRPT_LOG_DEBUG_STREAM("Adding ICP Constraints for nodeID: " << curr_nodeID);
203 
204  std::set<TNodeID> nodes_set;
205  this->fetchNodeIDsForScanMatching(curr_nodeID, &nodes_set);
206 
207  // try adding ICP constraints with each node in the previous set
208  for (std::set<TNodeID>::const_iterator node_it = nodes_set.begin();
209  node_it != nodes_set.end(); ++node_it)
210  {
211  constraint_t rel_edge;
212  mrpt::slam::CICP::TReturnInfo icp_info;
213 
214  MRPT_LOG_DEBUG_STREAM(
215  "Fetching laser scan for nodes: " << *node_it << "==> "
216  << curr_nodeID);
217 
218  bool found_edge =
219  this->getICPEdge(*node_it, curr_nodeID, &rel_edge, &icp_info);
220  if (!found_edge) continue;
221 
222  // keep track of the recorded goodness values
223  // TODO - rethink on these condition.
224  if (!isnan(icp_info.goodness) ||
225  !approximatelyEqual(static_cast<double>(icp_info.goodness), 0.0))
226  {
227  m_laser_params.goodness_threshold_win.addNewMeasurement(
228  icp_info.goodness);
229  }
230  double goodness_thresh =
231  m_laser_params.goodness_threshold_win.getMedian() *
232  m_consec_icp_constraint_factor;
233  bool accept_goodness = icp_info.goodness > goodness_thresh;
234  MRPT_LOG_DEBUG_STREAM(
235  "Curr. Goodness: " << icp_info.goodness
236  << "|\t Threshold: " << goodness_thresh << " => "
237  << (accept_goodness ? "ACCEPT" : "REJECT"));
238 
239  // make sure that the suggested edge makes sense with regards to current
240  // graph config - check against the current position difference
241  bool accept_mahal_distance = this->mahalanobisDistanceOdometryToICPEdge(
242  *node_it, curr_nodeID, rel_edge);
243 
244  // criterion for registering a new node
245  if (accept_goodness && accept_mahal_distance)
246  {
247  this->registerNewEdge(*node_it, curr_nodeID, rel_edge);
248  }
249  }
250 
251  MRPT_END;
252 } // end of addScanMatchingEdges
253 template <class GRAPH_T>
254 bool CLoopCloserERD<GRAPH_T>::getICPEdge(
255  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
256  constraint_t* rel_edge, mrpt::slam::CICP::TReturnInfo* icp_info,
257  const TGetICPEdgeAdParams* ad_params /*=NULL*/)
258 {
259  MRPT_START;
260  ASSERTDEB_(rel_edge);
261  this->m_time_logger.enter("getICPEdge");
262 
263  using namespace mrpt::obs;
264  using namespace std;
265  using namespace mrpt::graphslam::detail;
266 
267  // fetch the relevant laser scans and poses of the nodeIDs
268  // If given in the additional params struct, use those values instead of
269  // searching in the class std::map(s)
270  CObservation2DRangeScan::Ptr from_scan, to_scan;
271  global_pose_t from_pose;
272  global_pose_t to_pose;
273 
274  MRPT_LOG_DEBUG_STREAM("****In getICPEdge method: ");
275  if (ad_params)
276  {
277  MRPT_LOG_DEBUG_STREAM(
278  "TGetICPEdgeAdParams:\n"
279  << ad_params->getAsString() << endl);
280  }
281 
282  // from-node parameters
283  const node_props_t* from_params =
284  ad_params ? &ad_params->from_params : NULL;
285  bool from_success = this->getPropsOfNodeID(
286  from, &from_pose, from_scan, from_params); // TODO
287  // to-node parameters
288  const node_props_t* to_params = ad_params ? &ad_params->to_params : NULL;
289  bool to_success = this->getPropsOfNodeID(to, &to_pose, to_scan, to_params);
290 
291  if (!from_success || !to_success)
292  {
293  MRPT_LOG_DEBUG_STREAM(
294  "Either node #"
295  << from << " or node #" << to
296  << " doesn't contain a valid LaserScan. Ignoring this...");
297  return false;
298  }
299 
300  // make use of initial node position difference for the ICP edge
301  // from_node pose
302  pose_t initial_estim;
303  if (ad_params)
304  {
305  initial_estim = ad_params->init_estim;
306  }
307  else
308  {
309  initial_estim = to_pose - from_pose;
310  }
311 
312  MRPT_LOG_DEBUG_STREAM(
313  "from_pose: " << from_pose << "| to_pose: " << to_pose
314  << "| init_estim: " << initial_estim);
315 
316  range_ops_t::getICPEdge(
317  *from_scan, *to_scan, rel_edge, &initial_estim, icp_info);
318  MRPT_LOG_DEBUG_STREAM("*************");
319 
320  this->m_time_logger.leave("getICPEdge");
321  return true;
322  MRPT_END;
323 } // end of getICPEdge
324 
325 template <class GRAPH_T>
326 bool CLoopCloserERD<GRAPH_T>::fillNodePropsFromGroupParams(
327  const mrpt::graphs::TNodeID& nodeID,
328  const std::map<mrpt::graphs::TNodeID, node_props_t>& group_params,
329  node_props_t* node_props)
330 {
331  ASSERTDEB_(node_props);
332 
333  // MRPT_LOG_DEBUG_STREAM(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
334  // MRPT_LOG_DEBUG_STREAM("Running fillNodePropsFromGroupParams for nodeID
335  // \""
336  //<< nodeID << "\"");
337 
338  // Make sure that the given nodeID exists in the group_params
339  typename std::map<mrpt::graphs::TNodeID, node_props_t>::const_iterator
340  search = group_params.find(nodeID);
341  bool res = false;
342  if (search == group_params.end())
343  {
344  // MRPT_LOG_DEBUG_STREAM("Operation failed.");
345  }
346  else
347  {
348  *node_props = search->second;
349  res = true;
350  // MRPT_LOG_DEBUG_STREAM("Properties filled: " <<
351  // node_props->getAsString());
352  }
353 
354  // MRPT_LOG_DEBUG_STREAM("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
355  return res;
356 }
357 
358 template <class GRAPH_T>
359 bool CLoopCloserERD<GRAPH_T>::getPropsOfNodeID(
360  const mrpt::graphs::TNodeID& nodeID, global_pose_t* pose,
361  mrpt::obs::CObservation2DRangeScan::Ptr& scan,
362  const node_props_t* node_props /*=NULL*/) const
363 {
364  // make sure output instances are valid
365  ASSERTDEB_(pose);
366 
367  bool filled_pose = false;
368  bool filled_scan = false;
369 
370  if (node_props)
371  {
372  // Pose
373  if (node_props->pose != global_pose_t())
374  {
375  *pose = node_props->pose;
376  filled_pose = true;
377  }
378  // LaserScan
379  if (node_props->scan)
380  {
381  scan = node_props->scan;
382  filled_scan = true;
383  }
384  }
385 
386  // TODO - What if the node_props->pose is indeed 0?
387  ASSERTDEBMSG_(
388  !(filled_pose ^ filled_scan),
389  format(
390  "Either BOTH or NONE of the filled_pose, filled_scan should be set."
391  "NodeID: [%lu]",
392  static_cast<unsigned long>(nodeID)));
393 
394  //
395  // fill with class data if not yet available
396  //
397  if (!filled_pose)
398  {
399  // fill with class data if not yet available
400  typename GRAPH_T::global_poses_t::const_iterator search =
401  this->m_graph->nodes.find(nodeID);
402  if (search != this->m_graph->nodes.end())
403  {
404  *pose = search->second;
405  filled_pose = true;
406  }
407  else
408  {
409  MRPT_LOG_WARN_STREAM("pose not found for nodeID: " << nodeID);
410  }
411  }
412  if (!filled_scan)
413  {
414  typename nodes_to_scans2D_t::const_iterator search =
415  this->m_nodes_to_laser_scans2D.find(nodeID);
416  if (search != this->m_nodes_to_laser_scans2D.end())
417  {
418  scan = search->second;
419  filled_scan = true;
420  }
421  }
422 
423  return filled_pose && filled_scan;
424 }
425 
426 template <class GRAPH_T>
427 void CLoopCloserERD<GRAPH_T>::checkPartitionsForLC(
428  partitions_t* partitions_for_LC)
429 {
430  MRPT_START;
431  this->m_time_logger.enter("LoopClosureEvaluation");
432 
433  using namespace std;
434  using namespace mrpt;
435 
436  ASSERTDEB_(partitions_for_LC);
437  partitions_for_LC->clear();
438 
439  // keep track of the previous nodes list of every partition. If this is not
440  // changed - do not mark it as potential for loop closure
441  map<int, std::vector<uint32_t>>::iterator finder;
442  // reset the previous list if full partitioning was issued
443  if (m_partitions_full_update)
444  {
445  m_partitionID_to_prev_nodes_list.clear();
446  }
447 
448  int partitionID = 0;
449  // for every partition...
450  for (partitions_t::const_iterator partitions_it = m_curr_partitions.begin();
451  partitions_it != m_curr_partitions.end();
452  ++partitions_it, ++partitionID)
453  {
454  // check whether the last registered node is in the currently traversed
455  // partition - if not, ignore it.
456  if (m_lc_params.LC_check_curr_partition_only)
457  {
458  bool curr_node_in_curr_partition =
459  ((find(
460  partitions_it->begin(), partitions_it->end(),
461  this->m_graph->nodeCount() - 1)) != partitions_it->end());
462  if (!curr_node_in_curr_partition)
463  {
464  continue;
465  }
466  }
467 
468  // keep track of the previous nodes list
469  finder = m_partitionID_to_prev_nodes_list.find(partitionID);
470  if (finder == m_partitionID_to_prev_nodes_list.end())
471  {
472  // nodes list is not registered yet
473  m_partitionID_to_prev_nodes_list.insert(
474  make_pair(partitionID, *partitions_it));
475  }
476  else
477  {
478  if (*partitions_it == finder->second)
479  {
480  // MRPT_LOG_DEBUG_STREAM("Partition " << partitionID
481  //<< " remained unchanged. ");
482  continue; // same list as before.. no need to check this...
483  }
484  else
485  { // list was changed - update the previous nodes list
486  // MRPT_LOG_DEBUG_STREAM("Partition " << partitionID << "
487  // CHANGED. ");
488  finder->second = *partitions_it;
489  }
490  }
491 
492  // investigate each partition
493  int curr_node_index = 1;
494  size_t prev_nodeID = *(partitions_it->begin());
495  for (std::vector<uint32_t>::const_iterator
496  it = partitions_it->begin() + 1;
497  it != partitions_it->end(); ++it, ++curr_node_index)
498  {
499  size_t curr_nodeID = *it;
500 
501  // are there consecutive nodes with large difference inside this
502  // partition? Are these nodes enough to consider LC?
503  if ((curr_nodeID - prev_nodeID) > m_lc_params.LC_min_nodeid_diff)
504  {
505  // there is at least one divergent node..
506 
507  int num_after_nodes = partitions_it->size() - curr_node_index;
508  int num_before_nodes = partitions_it->size() - num_after_nodes;
509  if (num_after_nodes >= m_lc_params.LC_min_remote_nodes &&
510  num_before_nodes >= m_lc_params.LC_min_remote_nodes)
511  { // at least X LC nodes
512  MRPT_LOG_WARN_STREAM(
513  "Found potential loop closures:"
514  << endl
515  << "\tPartitionID: " << partitionID << endl
516  << "\tPartition: "
517  << mrpt::containers::getSTLContainerAsString(
518  *partitions_it)
519  .c_str()
520  << endl
521  << "\t" << prev_nodeID << " ==> " << curr_nodeID << endl
522  << "\tNumber of LC nodes: " << num_after_nodes);
523  partitions_for_LC->push_back(*partitions_it);
524  break; // no need to check the rest of the nodes in this
525  // partition
526  }
527  }
528 
529  // update the previous node
530  prev_nodeID = curr_nodeID;
531  }
532  MRPT_LOG_DEBUG_STREAM(
533  "Successfully checked partition: " << partitionID);
534  }
535 
536  this->m_time_logger.leave("LoopClosureEvaluation");
537  MRPT_END;
538 }
539 
540 template <class GRAPH_T>
541 void CLoopCloserERD<GRAPH_T>::evaluatePartitionsForLC(
542  const partitions_t& partitions)
543 {
544  MRPT_START;
545  using namespace mrpt;
546  using namespace mrpt::graphslam::detail;
547  using namespace mrpt::math;
548  using namespace std;
549  this->m_time_logger.enter("LoopClosureEvaluation");
550 
551  if (partitions.size() == 0) return;
552 
553  MRPT_LOG_DEBUG_FMT(
554  "Evaluating partitions for loop closures...\n%s\n",
555  this->header_sep.c_str());
556 
557  // for each partition to be evaulated...
558  for (partitions_t::const_iterator p_it = partitions.begin();
559  p_it != partitions.end(); ++p_it)
560  {
561  std::vector<uint32_t> partition(*p_it);
562 
563  // split the partition to groups
564  std::vector<uint32_t> groupA, groupB;
565  this->splitPartitionToGroups(
566  partition, &groupA, &groupB,
567  /*max_nodes_in_group=*/5);
568 
569  // generate hypotheses pool
570  hypotsp_t hypots_pool;
571  this->generateHypotsPool(groupA, groupB, &hypots_pool);
572 
573  // compute the pair-wise consistency matrix
574  CMatrixDouble consist_matrix(hypots_pool.size(), hypots_pool.size());
575  this->generatePWConsistenciesMatrix(
576  groupA, groupB, hypots_pool, &consist_matrix);
577 
578  // evaluate resulting matrix - fill valid hypotheses
579  hypotsp_t valid_hypots;
580  this->evalPWConsistenciesMatrix(
581  consist_matrix, hypots_pool, &valid_hypots);
582 
583  // registering the indicated/valid hypotheses
584  if (valid_hypots.size())
585  {
586  MRPT_LOG_WARN_STREAM("Registering Hypotheses...");
587  for (typename hypotsp_t::iterator it = valid_hypots.begin();
588  it != valid_hypots.end(); ++it)
589  {
590  this->registerHypothesis(**it);
591  }
592  }
593  // delete all hypotheses - generated in the heap...
594  MRPT_LOG_DEBUG_STREAM("Deleting the generated hypotheses pool...");
595  for (typename hypotsp_t::iterator it = hypots_pool.begin();
596  it != hypots_pool.end(); ++it)
597  {
598  delete *it;
599  }
600  } // for each partition
601 
602  MRPT_LOG_DEBUG_STREAM("\n" << this->header_sep);
603  this->m_time_logger.leave("LoopClosureEvaluation");
604 
605  MRPT_END;
606 }
607 
608 template <class GRAPH_T>
609 void CLoopCloserERD<GRAPH_T>::evalPWConsistenciesMatrix(
610  const mrpt::math::CMatrixDouble& consist_matrix,
611  const hypotsp_t& hypots_pool, hypotsp_t* valid_hypots)
612 {
613  MRPT_START;
614  using namespace std;
615  using namespace mrpt::math;
616 
617  ASSERTDEB_(valid_hypots);
618  valid_hypots->clear();
619 
620  // evaluate the pair-wise consistency matrix
621  // compute dominant eigenvector
622  dynamic_vector<double> u;
623  bool valid_lambda_ratio = this->computeDominantEigenVector(
624  consist_matrix, &u,
625  /*use_power_method=*/false);
626  if (!valid_lambda_ratio) return;
627 
628  // cout << "Dominant eigenvector: " << u.transpose() << endl;
629 
630  // discretize the indicator vector - maximize the dot product of
631  // w_unit .* u
632  ASSERTDEB_(u.size());
633  dynamic_vector<double> w(u.size(), 0); // discretized indicator vector
634  double dot_product = 0;
635  for (int i = 0; i != w.size(); ++i)
636  {
637  // stream for debugging reasons
638  stringstream ss;
639 
640  // make the necessary change and see if the dot product increases
641  w(i) = 1;
642  double potential_dot_product =
643  ((w.transpose() * u) / w.squaredNorm()).value();
644  ss << mrpt::format(
645  "current: %f | potential_dot_product: %f", dot_product,
646  potential_dot_product);
647  if (potential_dot_product > dot_product)
648  {
649  ss << " ==> ACCEPT";
650  dot_product = potential_dot_product;
651  }
652  else
653  {
654  ss << " ==> REJECT";
655  w(i) = 0; // revert the change
656  }
657  ss << endl;
658  // MRPT_LOG_DEBUG_STREAM(ss.str());
659  }
660  cout << "Outcome of discretization: " << w.transpose() << endl;
661  // mrpt::system::pause();
662 
663  // Current hypothesis is to be registered.
664  if (!w.isZero())
665  {
666  for (int wi = 0; wi != w.size(); ++wi)
667  {
668  if (w(wi) == 1)
669  {
670  // search through the potential hypotheses, find the one with
671  // the
672  // correct ID and register it.
673  valid_hypots->push_back(this->findHypotByID(hypots_pool, wi));
674  }
675  }
676  }
677  // mrpt::system::pause();
678 
679  MRPT_END;
680 }
681 
682 template <class GRAPH_T>
683 void CLoopCloserERD<GRAPH_T>::splitPartitionToGroups(
684  std::vector<uint32_t>& partition, std::vector<uint32_t>* groupA,
685  std::vector<uint32_t>* groupB, int max_nodes_in_group /*=5*/)
686 {
687  MRPT_START;
688 
689  using namespace mrpt;
690  using namespace mrpt::math;
691  using mrpt::graphs::TNodeID;
692 
693  // assertions
694  ASSERTDEBMSG_(groupA, "Pointer to groupA is not valid");
695  ASSERTDEBMSG_(groupB, "Pointer to groupB is not valid");
696  ASSERTDEBMSG_(
697  max_nodes_in_group == -1 || max_nodes_in_group > 0,
698  format(
699  "Value %d not permitted for max_nodes_in_group"
700  "Either use a positive integer, "
701  "or -1 for non-restrictive partition size",
702  max_nodes_in_group));
703 
704  // find a large difference between successive nodeIDs - that's where the cut
705  // is going to be
706  TNodeID prev_nodeID = 0;
707  size_t index_to_split = 1;
708  for (std::vector<uint32_t>::const_iterator it = partition.begin() + 1;
709  it != partition.end(); ++it, ++index_to_split)
710  {
711  TNodeID curr_nodeID = *it;
712 
713  if ((curr_nodeID - prev_nodeID) > m_lc_params.LC_min_nodeid_diff)
714  {
715  break;
716  }
717  // update the last nodeID
718  prev_nodeID = curr_nodeID;
719  }
720  ASSERTDEB_(partition.size() > index_to_split);
721 
722  // Fill the groups
723  *groupA = std::vector<uint32_t>(
724  partition.begin(), partition.begin() + index_to_split);
725  *groupB = std::vector<uint32_t>(
726  partition.begin() + index_to_split, partition.end());
727  // limit the number of nodes in each group
728  if (max_nodes_in_group != -1)
729  {
730  // groupA
731  if (groupA->size() > static_cast<size_t>(max_nodes_in_group))
732  {
733  *groupA = std::vector<uint32_t>(
734  groupA->begin(), groupA->begin() + max_nodes_in_group);
735  }
736  // groupB
737  if (groupB->size() > static_cast<size_t>(max_nodes_in_group))
738  {
739  *groupB = std::vector<uint32_t>(
740  groupB->end() - max_nodes_in_group, groupB->end());
741  }
742  }
743 
744  // mrpt::system::pause();
745  MRPT_END;
746 }
747 
748 template <class GRAPH_T>
749 void CLoopCloserERD<GRAPH_T>::generateHypotsPool(
750  const std::vector<uint32_t>& groupA, const std::vector<uint32_t>& groupB,
751  hypotsp_t* generated_hypots,
752  const TGenerateHypotsPoolAdParams* ad_params /*=NULL*/)
753 {
754  MRPT_START;
755  using namespace mrpt;
756  using namespace mrpt::containers;
757 
758  ASSERTDEBMSG_(
759  generated_hypots,
760  "generateHypotsPool: Given hypotsp_t pointer is invalid.");
761  generated_hypots->clear();
762 
763  MRPT_LOG_DEBUG_STREAM("Generating hypotheses for groups: " << endl);
764  MRPT_LOG_DEBUG_STREAM(
765  "- groupA:\t" << getSTLContainerAsString(groupA)
766  << " - size: " << groupA.size() << endl);
767  MRPT_LOG_DEBUG_STREAM(
768  "- groupB:\t" << getSTLContainerAsString(groupB)
769  << " - size: " << groupB.size() << endl);
770 
771  // verify that the number of laserScans is the same as the number of poses
772  // if
773  // the TGenerateHyptsPoolAdParams is used
774  // formulate into function and pass std::vector<uint32_t> and
775  // ad_params->group
776  // TODO
777  if (ad_params)
778  {
779  const typename TGenerateHypotsPoolAdParams::group_t& params =
780  ad_params->groupA_params;
781  if (params.size())
782  {
783  size_t nodes_count = groupA.size();
784 
785  // map should have same size
786  ASSERTDEBMSG_(
787  nodes_count == params.size(),
788  format(
789  "Size mismatch between nodeIDs in group [%lu]"
790  " and corresponding properties map [%lu]",
791  nodes_count, params.size()));
792  }
793  }
794 
795  // use a hypothesis ID with which the consistency matrix will then be
796  // formed
797  int hypot_counter = 0;
798  int invalid_hypots = 0; // just for keeping track of them.
799  {
800  // iterate over all the nodes in both groups
801  for (std::vector<uint32_t>::const_iterator // B - from
802  b_it = groupB.begin();
803  b_it != groupB.end(); ++b_it)
804  {
805  for (std::vector<uint32_t>::const_iterator // A - to
806  a_it = groupA.begin();
807  a_it != groupA.end(); ++a_it)
808  {
809  // by default hypotheses will direct bi => ai; If the hypothesis
810  // is
811  // traversed the opposite way, take the opposite of the
812  // constraint
813  hypot_t* hypot = new hypot_t;
814  hypot->from = *b_it;
815  hypot->to = *a_it;
816  hypot->id = hypot_counter++;
817 
818  // [from] *b_it ====[edge]===> [to] *a_it
819 
820  // Fetch and set the pose and LaserScan of from, to nodeIDs
821  //
822  // even if icp_ad_params NULL, it will be handled appropriately
823  // by the
824  // getICPEdge fun.
825  // bool from_success, to_success;
826  TGetICPEdgeAdParams* icp_ad_params = NULL;
827  if (ad_params)
828  {
829  icp_ad_params = new TGetICPEdgeAdParams;
830  // from_success = fillNodePropsFromGroupParams(
831  fillNodePropsFromGroupParams(
832  *b_it, ad_params->groupB_params,
833  &icp_ad_params->from_params);
834  // to_success = fillNodePropsFromGroupParams(
835  fillNodePropsFromGroupParams(
836  *a_it, ad_params->groupA_params,
837  &icp_ad_params->to_params);
838 
839  // MRPT_LOG_DEBUG_STREAM(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>");
840  // MRPT_LOG_DEBUG_STREAM("From nodeID (other): " << *b_it);
841  // MRPT_LOG_DEBUG_STREAM("From params (other): "
842  //<< icp_ad_params->from_params.getAsString());
843  // MRPT_LOG_DEBUG_STREAM("from_success: " << (from_success?
844  // "TRUE" : "FALSE"));
845  // MRPT_LOG_DEBUG_STREAM("**********");
846  // MRPT_LOG_DEBUG_STREAM("To nodeID (own) : " << *a_it);
847  // MRPT_LOG_DEBUG_STREAM("To params (own) : "
848  //<< icp_ad_params->to_params.getAsString());
849  // MRPT_LOG_DEBUG_STREAM("to_success: " << (to_success?
850  // "TRUE" : "FALSE"));
851  // MRPT_LOG_DEBUG_STREAM("<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<");
852  }
853 
854  // fetch the ICP constraint bi => ai
855  mrpt::slam::CICP::TReturnInfo icp_info;
856  constraint_t edge;
857  bool found_edge = this->getICPEdge(
858  *b_it, *a_it, &edge, &icp_info, icp_ad_params);
859 
860  hypot->setEdge(edge);
861  hypot->goodness =
862  icp_info.goodness; // goodness related to the edge
863 
864  // Check if invalid
865  //
866  // Goodness Threshold
867  double goodness_thresh =
868  m_laser_params.goodness_threshold_win.getMedian() *
869  m_lc_icp_constraint_factor;
870  bool accept_goodness = icp_info.goodness > goodness_thresh;
871  MRPT_LOG_DEBUG_STREAM(
872  "generateHypotsPool:\nCurr. Goodness: "
873  << icp_info.goodness << "|\t Threshold: " << goodness_thresh
874  << " => " << (accept_goodness ? "ACCEPT" : "REJECT")
875  << endl);
876 
877  if (!found_edge || !accept_goodness)
878  {
879  hypot->is_valid = false;
880  invalid_hypots++;
881  }
882  generated_hypots->push_back(hypot);
883  MRPT_LOG_DEBUG_STREAM(hypot->getAsString());
884 
885  // delete pointer to getICPEdge additional parameters if they
886  // were
887  // initialized
888  delete icp_ad_params;
889  }
890  }
891  MRPT_LOG_DEBUG_STREAM(
892  "Generated pool of hypotheses...\tsize = "
893  << generated_hypots->size()
894  << "\tinvalid hypotheses: " << invalid_hypots);
895  }
896  // mrpt::system::pause();
897 
898  MRPT_END;
899 } // end of generateHypotsPool
900 
901 template <class GRAPH_T>
902 bool CLoopCloserERD<GRAPH_T>::computeDominantEigenVector(
903  const mrpt::math::CMatrixDouble& consist_matrix,
904  mrpt::math::dynamic_vector<double>* eigvec, bool use_power_method /*=true*/)
905 {
906  MRPT_START;
907  using namespace mrpt;
908  using namespace mrpt::math;
909  using namespace std;
910  ASSERTDEB_(eigvec);
911 
912  this->m_time_logger.enter("DominantEigenvectorComputation");
913 
914  double lambda1, lambda2; // eigenvalues to use
915  bool is_valid_lambda_ratio = false;
916 
917  if (use_power_method)
918  {
919  THROW_EXCEPTION(
920  "\nPower method for computing the first two "
921  "eigenvectors/eigenvalues hasn't been implemented yet\n");
922  }
923  else
924  { // call to eigenVectors method
925  CMatrixDouble eigvecs, eigvals;
926  consist_matrix.eigenVectors(eigvecs, eigvals);
927 
928  // assert that the eivenvectors, eigenvalues, consistency matrix are of
929  // the
930  // same size
931  ASSERTDEBMSG_(
932  eigvecs.size() == eigvals.size() &&
933  consist_matrix.size() == eigvals.size(),
934  mrpt::format(
935  "Size of eigvecs \"%lu\","
936  "eigvalues \"%lu\","
937  "consist_matrix \"%lu\" don't match",
938  static_cast<unsigned long>(eigvecs.size()),
939  static_cast<unsigned long>(eigvals.size()),
940  static_cast<unsigned long>(consist_matrix.size())));
941 
942  eigvecs.extractCol(eigvecs.cols() - 1, *eigvec);
943  lambda1 = eigvals(eigvals.rows() - 1, eigvals.cols() - 1);
944  lambda2 = eigvals(eigvals.rows() - 2, eigvals.cols() - 2);
945  }
946 
947  // I don't care about the sign of the eigenvector element
948  for (int i = 0; i != eigvec->size(); ++i)
949  {
950  (*eigvec)(i) = abs((*eigvec)(i));
951  }
952 
953  // check the ratio of the two eigenvalues - reject hypotheses set if ratio
954  // smaller than threshold
955  if (approximatelyEqual(0.0, lambda2, /**limit = */ 0.00001))
956  {
957  MRPT_LOG_ERROR_STREAM(
958  "Bad lambda2 value: "
959  << lambda2 << " => Skipping current evaluation." << endl);
960  return false;
961  }
962  double curr_lambda_ratio = lambda1 / lambda2;
963  MRPT_LOG_DEBUG_STREAM(
964  "lambda1 = " << lambda1 << " | lambda2 = " << lambda2
965  << "| ratio = " << curr_lambda_ratio << endl);
966 
967  is_valid_lambda_ratio =
968  (curr_lambda_ratio > m_lc_params.LC_eigenvalues_ratio_thresh);
969 
970  this->m_time_logger.leave("DominantEigenvectorComputation");
971  return is_valid_lambda_ratio;
972 
973  MRPT_END;
974 } // end of computeDominantEigenVector
975 
976 template <class GRAPH_T>
977 void CLoopCloserERD<GRAPH_T>::generatePWConsistenciesMatrix(
978  const std::vector<uint32_t>& groupA, const std::vector<uint32_t>& groupB,
979  const hypotsp_t& hypots_pool, mrpt::math::CMatrixDouble* consist_matrix,
980  const paths_t* groupA_opt_paths /*=NULL*/,
981  const paths_t* groupB_opt_paths /*=NULL*/)
982 {
983  MRPT_START;
984 
985  using namespace mrpt;
986  using namespace mrpt::math;
987  using namespace mrpt::containers;
988  using namespace std;
989  using mrpt::graphs::TNodeID;
990 
991  ASSERTDEBMSG_(
992  consist_matrix, "Invalid pointer to the Consistency matrix is given");
993  ASSERTDEBMSG_(
994  static_cast<unsigned long>(consist_matrix->rows()) ==
995  hypots_pool.size() &&
996  static_cast<unsigned long>(consist_matrix->rows()) ==
997  hypots_pool.size(),
998  "Consistency matrix dimensions aren't equal to the hypotheses pool "
999  "size");
1000 
1001  MRPT_LOG_DEBUG_STREAM(
1002  ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
1003  << endl
1004  << "In generatePWConsistencyMatrix:\n"
1005  << "\tgroupA: " << getSTLContainerAsString(groupA) << endl
1006  << "\tgroupB: " << getSTLContainerAsString(groupB) << endl
1007  << "\tHypots pool Size: " << hypots_pool.size());
1008 
1009  // b1
1010  for (std::vector<uint32_t>::const_iterator b1_it = groupB.begin();
1011  b1_it != groupB.end(); ++b1_it)
1012  {
1013  TNodeID b1 = *b1_it;
1014 
1015  // b2
1016  for (std::vector<uint32_t>::const_iterator b2_it = b1_it + 1;
1017  b2_it != groupB.end(); ++b2_it)
1018  {
1019  TNodeID b2 = *b2_it;
1020 
1021  // a1
1022  for (std::vector<uint32_t>::const_iterator a1_it = groupA.begin();
1023  a1_it != groupA.end(); ++a1_it)
1024  {
1025  TNodeID a1 = *a1_it;
1026  hypot_t* hypot_b2_a1 =
1027  this->findHypotByEnds(hypots_pool, b2, a1);
1028  // MRPT_LOG_DEBUG_STREAM("hypot_b2_a1: " <<
1029  // hypot_b2_a1->getAsString());
1030 
1031  // a2
1032  for (std::vector<uint32_t>::const_iterator a2_it = a1_it + 1;
1033  a2_it != groupA.end(); ++a2_it)
1034  {
1035  TNodeID a2 = *a2_it;
1036  hypot_t* hypot_b1_a2 =
1037  this->findHypotByEnds(hypots_pool, b1, a2);
1038  // MRPT_LOG_DEBUG_STREAM("hypot_b1_a2: " <<
1039  // hypot_b1_a2->getAsString());
1040 
1041  double consistency;
1042 
1043  // compute consistency element
1044  if (hypot_b2_a1->is_valid && hypot_b1_a2->is_valid)
1045  {
1046  // extract vector of hypotheses that connect the given
1047  // nodes,
1048  // instead of passing the whole hypothesis pool.
1049  hypotsp_t extracted_hypots;
1050  extracted_hypots.push_back(hypot_b2_a1);
1051  extracted_hypots.push_back(hypot_b1_a2);
1052 
1053  paths_t* curr_opt_paths = NULL;
1054  if (groupA_opt_paths || groupB_opt_paths)
1055  { // fill curr_opt_paths
1056  curr_opt_paths = new paths_t();
1057  }
1058 
1059  // decide on additional optimal paths
1060  if (curr_opt_paths)
1061  {
1062  // groupA
1063  if (groupA_opt_paths)
1064  { // a1 -> a2 optimal path
1065  const path_t* p = this->findPathByEnds(
1066  *groupA_opt_paths, a1, a2,
1067  /*throw_exc=*/true);
1068  curr_opt_paths->push_back(*p);
1069  }
1070  else
1071  { // empty
1072  curr_opt_paths->push_back(path_t());
1073  }
1074 
1075  if (groupB_opt_paths)
1076  { // b1 -> b2 optimal path
1077  const path_t* p = this->findPathByEnds(
1078  *groupB_opt_paths, b1, b2,
1079  /*throw_exc=*/true);
1080  curr_opt_paths->push_back(*p);
1081  }
1082  else
1083  { // empty
1084  curr_opt_paths->push_back(path_t());
1085  }
1086  }
1087 
1088  consistency = this->generatePWConsistencyElement(
1089  a1, a2, b1, b2, extracted_hypots, curr_opt_paths);
1090 
1091  delete curr_opt_paths;
1092  }
1093  else
1094  { // null those that don't look good
1095  consistency = 0;
1096  }
1097 
1098  // MRPT_LOG_DEBUG_STREAM(
1099  //"Adding hypothesis consistency for nodeIDs: "
1100  //<< "[b1] " << b1 << ", [b2] -> " << b2
1101  //<< ", [a1] -> " << a1 << ", [a2] -> " << a2
1102  //<< " ==> " << consistency << endl);
1103 
1104  // fill the PW consistency matrix corresponding element -
1105  // symmetrical
1106  int id1 = hypot_b2_a1->id;
1107  int id2 = hypot_b1_a2->id;
1108 
1109  (*consist_matrix)(id1, id2) = consistency;
1110  (*consist_matrix)(id2, id1) = consistency;
1111 
1112  // MRPT_LOG_DEBUG_STREAM("id1 = " << id1 << "\t|"
1113  //<< "id2 = " << id2 << "\t|"
1114  //<< "consistency = " << consistency);
1115  }
1116  }
1117  }
1118  }
1119 
1120  // MRPT_LOG_WARN_STREAM("Consistency matrix:" << endl
1121  //<< this->header_sep << endl
1122  //<< *consist_matrix << endl);
1123 
1124  MRPT_END;
1125 } // end of generatePWConsistenciesMatrix
1126 
1127 template <class GRAPH_T>
1128 double CLoopCloserERD<GRAPH_T>::generatePWConsistencyElement(
1129  const mrpt::graphs::TNodeID& a1, const mrpt::graphs::TNodeID& a2,
1130  const mrpt::graphs::TNodeID& b1, const mrpt::graphs::TNodeID& b2,
1131  const hypotsp_t& hypots, const paths_t* opt_paths /*=NULL*/)
1132 {
1133  MRPT_START;
1134  using namespace std;
1135  using namespace mrpt;
1136  using namespace mrpt::math;
1137  using namespace mrpt::graphslam;
1138  using namespace mrpt::graphslam::detail;
1139 
1140  // MRPT_LOG_DEBUG_STREAM("In generatePWConsistencyElement.\n"
1141  //<< "\t[" << a1 << ", " << a2 << "]\n"
1142  //<< "\t[" << b1 << ", " << b2 << "]");
1143  // MRPT_LOG_DEBUG_STREAM("a1->a2 optimal path:"
1144  //<< (opt_paths && !opt_paths->begin()->isEmpty()?
1145  // opt_paths->begin()->getAsString() :
1146  //"NONE"));
1147  // MRPT_LOG_DEBUG_STREAM("b1->b2 optimal path: "
1148  //<< (opt_paths && !opt_paths->rbegin()->isEmpty()?
1149  // opt_paths->rbegin()->getAsString() :
1150  //"NONE"));
1151 
1152  // standard size assertions
1153  ASSERTDEB_(hypots.size() == 2);
1154  if (opt_paths)
1155  {
1156  ASSERTDEB_(opt_paths->size() == 2);
1157  }
1158 
1159  //
1160  // get the Dijkstra links
1161  //
1162  // a1 ==> a2
1163  const path_t* path_a1_a2;
1164  if (!opt_paths || opt_paths->begin()->isEmpty())
1165  {
1166  MRPT_LOG_DEBUG_STREAM(
1167  "Running dijkstra [a1] " << a1 << " => [a2] " << a2);
1168  execDijkstraProjection(/*starting_node=*/a1, /*ending_node=*/a2);
1169  path_a1_a2 = this->queryOptimalPath(a2);
1170  }
1171  else
1172  { // fetch the path from the opt_paths arg
1173  // TODO dubious practice
1174  path_a1_a2 = &(*opt_paths->begin());
1175  }
1176  ASSERTDEB_(path_a1_a2);
1177  path_a1_a2->assertIsBetweenNodeIDs(/*start=*/a1, /*end*/ a2);
1178 
1179  // b1 ==> b2
1180  const path_t* path_b1_b2;
1181  if (!opt_paths || opt_paths->rend()->isEmpty())
1182  {
1183  MRPT_LOG_DEBUG_STREAM(
1184  "Running djkstra [b1] " << b1 << " => [b2] " << b2);
1185  execDijkstraProjection(/*starting_node=*/b1, /*ending_node=*/b2);
1186  path_b1_b2 = this->queryOptimalPath(b2);
1187  }
1188  else
1189  { // fetch the path from the opt_paths arg
1190  path_b1_b2 = &(*opt_paths->rbegin());
1191  }
1192  ASSERTDEB_(path_b1_b2);
1193  path_b1_b2->assertIsBetweenNodeIDs(/*start=*/b1, /*end*/ b2);
1194  // get the edges of the hypotheses
1195  // by default hypotheses are stored bi => ai
1196  //
1197  // Backwards edge: a2=>b1
1198  hypot_t* hypot_b1_a2 = this->findHypotByEnds(hypots, b1, a2);
1199  // forward edge b2=>a1
1200  hypot_t* hypot_b2_a1 = this->findHypotByEnds(hypots, b2, a1);
1201 
1202  // Composition of Poses
1203  // Order : a1 ==> a2 ==> b1 ==> b2 ==> a1
1204  constraint_t res_transform(path_a1_a2->curr_pose_pdf);
1205  res_transform += hypot_b1_a2->getInverseEdge();
1206  res_transform += path_b1_b2->curr_pose_pdf;
1207  res_transform += hypot_b2_a1->getEdge();
1208 
1209  MRPT_LOG_DEBUG_STREAM(
1210  "\n-----------Resulting Transformation----------- Hypots: #"
1211  << hypot_b1_a2->id << ", #" << hypot_b2_a1->id << endl
1212  << "a1 --> a2 => b1 --> b2 => a1: " << a1 << " --> " << a2 << " => "
1213  << b1 << " --> " << b2 << " => " << a1 << endl
1214  << res_transform << endl
1215  << endl
1216  << "DIJKSTRA: " << a1 << " --> " << a2 << ": "
1217  << path_a1_a2->curr_pose_pdf << endl
1218  << "DIJKSTRA: " << b1 << " --> " << b2 << ": "
1219  << path_b1_b2->curr_pose_pdf << endl
1220  << "hypot_b1_a2(inv):\n"
1221  << hypot_b1_a2->getInverseEdge() << endl
1222  << "hypot_b2_a1:\n"
1223  << hypot_b2_a1->getEdge() << endl);
1224 
1225  // get the vector of the corresponding transformation - [x, y, phi] form
1226  dynamic_vector<double> T;
1227  res_transform.getMeanVal().getAsVector(T);
1228 
1229  // information matrix
1230  CMatrixDouble33 cov_mat;
1231  res_transform.getCovariance(cov_mat);
1232 
1233  // there has to be an error with the initial Olson formula - p.15.
1234  // There must be a minus in the exponent and the covariance matrix instead
1235  // of
1236  // the information matrix.
1237  double exponent = (-T.transpose() * cov_mat * T).value();
1238  double consistency_elem = exp(exponent);
1239 
1240  // cout << "T = " << endl << T << endl;
1241  // cout << "exponent = " << exponent << endl;
1242  // cout << "consistency_elem = " << consistency_elem << endl;
1243  // mrpt::system::pause();
1244 
1245  return consistency_elem;
1246  MRPT_END;
1247 } // end of generatePWConsistencyElement
1248 
1249 template <class GRAPH_T>
1250 const mrpt::graphslam::TUncertaintyPath<GRAPH_T>*
1251  CLoopCloserERD<GRAPH_T>::findPathByEnds(
1252  const paths_t& vec_paths, const mrpt::graphs::TNodeID& src,
1253  const mrpt::graphs::TNodeID& dst, bool throw_exc /*=true*/)
1254 {
1255  using namespace mrpt;
1256 
1257  ASSERTDEB_(vec_paths.size());
1258  const path_t* res = NULL;
1259 
1260  for (typename paths_t::const_iterator cit = vec_paths.begin();
1261  cit != vec_paths.end(); ++cit)
1262  {
1263  if (cit->getSource() == src && cit->getDestination() == dst)
1264  {
1265  res = &(*cit);
1266  break;
1267  }
1268  }
1269  if (throw_exc && !res)
1270  {
1271  THROW_EXCEPTION(
1272  format(
1273  "Path for %lu => %lu is not found. Exiting...\n",
1274  static_cast<unsigned long>(src),
1275  static_cast<unsigned long>(dst)));
1276  }
1277 
1278  return res;
1279 }
1280 
1281 template <class GRAPH_T>
1282 mrpt::graphs::detail::THypothesis<GRAPH_T>*
1283  CLoopCloserERD<GRAPH_T>::findHypotByEnds(
1284  const hypotsp_t& vec_hypots, const mrpt::graphs::TNodeID& from,
1285  const mrpt::graphs::TNodeID& to, bool throw_exc /*=true*/)
1286 {
1287  using namespace mrpt::graphslam::detail;
1288  using namespace std;
1289 
1290  for (typename hypotsp_t::const_iterator v_cit = vec_hypots.begin();
1291  v_cit != vec_hypots.end(); v_cit++)
1292  {
1293  if ((*v_cit)->hasEnds(from, to))
1294  {
1295  // cout << "findHypotByEnds: Found hypot " << from
1296  //<< " => " << to << " : " << (*v_cit)->getAsString() << endl;
1297  return *v_cit;
1298  }
1299  }
1300 
1301  // not found.
1302  if (throw_exc)
1303  {
1304  throw mrpt::graphs::HypothesisNotFoundException(from, to);
1305  }
1306  else
1307  {
1308  return NULL;
1309  }
1310 } // end of findHypotByEnds
1311 
1312 template <class GRAPH_T>
1313 mrpt::graphs::detail::THypothesis<GRAPH_T>*
1314  CLoopCloserERD<GRAPH_T>::findHypotByID(
1315  const hypotsp_t& vec_hypots, const size_t& id, bool throw_exc /*=true*/)
1316 {
1317  using namespace mrpt::graphslam::detail;
1318 
1319  for (typename hypotsp_t::const_iterator v_cit = vec_hypots.begin();
1320  v_cit != vec_hypots.end(); v_cit++)
1321  {
1322  if ((*v_cit)->id == id)
1323  {
1324  return *v_cit;
1325  }
1326  }
1327 
1328  // not found.
1329  if (throw_exc)
1330  {
1331  throw mrpt::graphs::HypothesisNotFoundException(id);
1332  }
1333  else
1334  {
1335  return NULL;
1336  }
1337 }
1338 
1339 template <class GRAPH_T>
1340 void CLoopCloserERD<GRAPH_T>::execDijkstraProjection(
1341  mrpt::graphs::TNodeID starting_node /*=0*/,
1342  mrpt::graphs::TNodeID ending_node /*=INVALID_NODEID*/)
1343 {
1344  MRPT_START;
1345  using namespace std;
1346  using namespace mrpt;
1347  using namespace mrpt::math;
1348  using mrpt::graphs::TNodeID;
1349 
1350  // for the full algorithm see
1351  // - Recognizing places using spectrally clustered local matches - E.Olson,
1352  // p.6
1353 
1354  this->m_time_logger.enter("Dijkstra Projection");
1355  const std::string dijkstra_end =
1356  "----------- Done with Dijkstra Projection... ----------";
1357 
1358  // ending_node is either INVALID_NODEID or one of the already registered
1359  // nodeIDs
1360  ASSERTDEB_(
1361  ending_node == INVALID_NODEID ||
1362  (ending_node >= 0 && ending_node < this->m_graph->nodeCount()));
1363  ASSERTDEBMSG_(
1364  starting_node != ending_node, "Starting and Ending nodes coincede");
1365 
1366  // debugging message
1367  stringstream ss_debug("");
1368  ss_debug << "Executing Dijkstra Projection: " << starting_node << " => ";
1369  if (ending_node == INVALID_NODEID)
1370  {
1371  ss_debug << "..." << endl;
1372  }
1373  else
1374  {
1375  ss_debug << ending_node << endl;
1376  }
1377 
1378  if (this->m_graph->nodeCount() < m_dijkstra_node_count_thresh)
1379  {
1380  return;
1381  }
1382 
1383  // keep track of the nodes that I have visited
1384  std::vector<bool> visited_nodes(this->m_graph->nodeCount(), false);
1385  m_node_optimal_paths.clear();
1386 
1387  // get the neighbors of each node
1388  std::map<TNodeID, std::set<TNodeID>> neighbors_of;
1389  this->m_graph->getAdjacencyMatrix(neighbors_of);
1390 
1391  // initialize a pool of TUncertaintyPaths - draw the minimum-uncertainty
1392  // path during
1393  // execution
1394  std::set<path_t*> pool_of_paths;
1395  // get the edge to each one of the neighboring nodes of the starting node
1396  std::set<TNodeID> starting_node_neighbors(neighbors_of.at(starting_node));
1397  for (std::set<TNodeID>::const_iterator n_it =
1398  starting_node_neighbors.begin();
1399  n_it != starting_node_neighbors.end(); ++n_it)
1400  {
1401  path_t* path_between_neighbors = new path_t();
1402  this->getMinUncertaintyPath(
1403  starting_node, *n_it, path_between_neighbors);
1404 
1405  pool_of_paths.insert(path_between_neighbors);
1406  }
1407  // just visited the first node
1408  visited_nodes.at(starting_node) = true;
1409 
1410  //// TODO Remove these - >>>>>>>>>>>>>>>>>>>>
1411  //// printing the pool for verification
1412  // cout << "Pool of Paths: " << endl;
1413  // for (typename std::set<path_t*>::const_iterator it =
1414  // pool_of_paths.begin();
1415  // it != pool_of_paths.end(); ++it) {
1416  // printSTLContainer((*it)->nodes_traversed);
1417  //}
1418  // cout << "------ Done with the starting node ... ------" << endl;
1419  // int iters = 0;
1420  //// TODO Remove these - <<<<<<<<<<<<<<<<<<<<< vvvUNCOMMENT BELOW AS WELLvvv
1421 
1422  while (true)
1423  {
1424  // if there is at least one false, exit loop
1425  for (std::vector<bool>::const_iterator it = visited_nodes.begin();
1426  it != visited_nodes.end(); ++it)
1427  {
1428  if (!*it)
1429  {
1430  break;
1431  }
1432  }
1433 
1434  // if an ending nodeID has been specified, end the method when the path
1435  // to
1436  // it is found.
1437  if (ending_node != INVALID_NODEID)
1438  {
1439  if (visited_nodes.at(ending_node))
1440  {
1441  // MRPT_LOG_DEBUG_STREAM(dijkstra_end);
1442  this->m_time_logger.leave("Dijkstra Projection");
1443  return;
1444  }
1445  }
1446 
1447  path_t* optimal_path = this->popMinUncertaintyPath(&pool_of_paths);
1448  TNodeID dest = optimal_path->getDestination();
1449 
1450  //// TODO Remove these - >>>>>>>>>>>>>>>>>>>> ^^^UNCOMMENT ABOVE AS
1451  /// WELL^^^
1452  // cout << iters << " " << std::string(40, '>') << endl;
1453  // cout << "current path Destination: " << dest << endl;
1454  //// printing the pool for verification
1455  // cout << "Pool of Paths: " << endl;
1456  // for (typename std::set<path_t*>::const_iterator
1457  // it = pool_of_paths.begin();
1458  // it != pool_of_paths.end(); ++it) {
1459  // printSTLContainer((*it)->nodes_traversed);
1460  //}
1461  // cout << "Nodes visited: " << endl;
1462  // std::vector<int> tmp_vec;
1463  // for (int i = 0; i != visited_nodes.size(); ++i) {
1464  // tmp_vec.push_back(i);
1465  //}
1466  // printSTLContainer(tmp_vec); cout << endl; // indices of numbers
1467  // printSTLContainer(visited_nodes); // actual flags
1468  // cout << std::string(40, '<') << " " << iters++ << endl;
1469  // mrpt::system::pause();
1470  //// TODO Remove these - <<<<<<<<<<<<<<<<<<<<<
1471 
1472  if (!visited_nodes.at(dest))
1473  {
1474  m_node_optimal_paths[dest] = optimal_path;
1475  visited_nodes.at(dest) = true;
1476 
1477  // for all the edges leaving this node .. compose the transforms
1478  // with the
1479  // current pool of paths.
1480  this->addToPaths(
1481  &pool_of_paths, *optimal_path, neighbors_of.at(dest));
1482  }
1483  }
1484 
1485  // MRPT_LOG_DEBUG_STREAM(dijkstra_end);
1486  this->m_time_logger.leave("Dijkstra Projection");
1487  MRPT_END;
1488 }
1489 
1490 template <class GRAPH_T>
1491 void CLoopCloserERD<GRAPH_T>::addToPaths(
1492  std::set<path_t*>* pool_of_paths, const path_t& current_path,
1493  const std::set<mrpt::graphs::TNodeID>& neighbors) const
1494 {
1495  MRPT_START;
1496  using namespace std;
1497  using namespace mrpt::graphslam;
1498  using mrpt::graphs::TNodeID;
1499 
1500  TNodeID node_to_append_from = current_path.getDestination();
1501 
1502  // compose transforms for every neighbor of node_to_append_from *except*
1503  // for the link connecting node_to_append_from and the second to last node
1504  // in
1505  // the current_path
1506  TNodeID second_to_last_node = current_path.nodes_traversed.rbegin()[1];
1507  for (std::set<TNodeID>::const_iterator neigh_it = neighbors.begin();
1508  neigh_it != neighbors.end(); ++neigh_it)
1509  {
1510  if (*neigh_it == second_to_last_node) continue;
1511 
1512  // get the path between node_to_append_from, *node_it
1513  path_t path_between_nodes;
1514  this->getMinUncertaintyPath(
1515  node_to_append_from, *neigh_it, &path_between_nodes);
1516 
1517  // format the path to append
1518  path_t* path_to_append = new path_t();
1519  *path_to_append = current_path;
1520  *path_to_append += path_between_nodes;
1521 
1522  pool_of_paths->insert(path_to_append);
1523  }
1524 
1525  MRPT_END;
1526 }
1527 
1528 template <class GRAPH_T>
1529 typename mrpt::graphslam::TUncertaintyPath<GRAPH_T>* CLoopCloserERD<
1530  GRAPH_T>::queryOptimalPath(const mrpt::graphs::TNodeID node) const
1531 {
1532  using namespace std;
1533 
1534  path_t* path = NULL;
1535  typename std::map<mrpt::graphs::TNodeID, path_t*>::const_iterator search;
1536  search = m_node_optimal_paths.find(node);
1537  if (search != m_node_optimal_paths.end())
1538  {
1539  path = search->second;
1540  }
1541 
1542  // MRPT_LOG_DEBUG_STREAM("Queried optimal path for nodeID: " << node
1543  //<< " ==> Path: " << (path? "Found" : "NOT Found"));
1544  return path;
1545 }
1546 
1547 template <class GRAPH_T>
1548 void CLoopCloserERD<GRAPH_T>::getMinUncertaintyPath(
1549  const mrpt::graphs::TNodeID from, const mrpt::graphs::TNodeID to,
1550  path_t* path_between_nodes) const
1551 {
1552  MRPT_START;
1553  using namespace mrpt::math;
1554  using namespace std;
1555 
1556  ASSERTDEBMSG_(
1557  this->m_graph->edgeExists(from, to) ||
1558  this->m_graph->edgeExists(to, from),
1559  mrpt::format(
1560  "\nEdge between the provided nodeIDs"
1561  "(%lu <-> %lu) does not exist\n",
1562  from, to));
1563  ASSERTDEB_(path_between_nodes);
1564 
1565  // cout << "getMinUncertaintyPath: " << from << " => " << to << endl;
1566 
1567  // don't add to the path_between_nodes, just fill it in afterwards
1568  path_between_nodes->clear();
1569 
1570  // iterate over all the edges, ignore the ones that are all 0s - find the
1571  // one that is with the lowest uncertainty
1572  double curr_determinant = 0;
1573  // forward edges from -> to
1574  std::pair<edges_citerator, edges_citerator> fwd_edges_pair =
1575  this->m_graph->getEdges(from, to);
1576 
1577  // cout << "Forward edges: " << endl;
1578  // for (edges_citerator e_it = fwd_edges_pair.first; e_it !=
1579  // fwd_edges_pair.second;
1580  //++e_it) {
1581  // cout << e_it->second << endl;
1582  //}
1583 
1584  for (edges_citerator edges_it = fwd_edges_pair.first;
1585  edges_it != fwd_edges_pair.second; ++edges_it)
1586  {
1587  // operate on a temporary object instead of the real edge - otherwise
1588  // function is non-const
1589  constraint_t curr_edge;
1590  curr_edge.copyFrom(edges_it->second);
1591 
1592  // is it all 0s?
1593  CMatrixDouble33 inf_mat;
1594  curr_edge.getInformationMatrix(inf_mat);
1595 
1596  if (inf_mat == CMatrixDouble33() || std::isnan(inf_mat(0, 0)))
1597  {
1598  inf_mat.unit();
1599  curr_edge.cov_inv = inf_mat;
1600  }
1601 
1602  path_t curr_path(from); // set the starting node
1603  curr_path.addToPath(to, curr_edge);
1604 
1605  // update the resulting path_between_nodes if its determinant is smaller
1606  // than the determinant of the current path_between_nodes
1607  if (curr_determinant < curr_path.getDeterminant())
1608  {
1609  curr_determinant = curr_path.getDeterminant();
1610  *path_between_nodes = curr_path;
1611  }
1612  }
1613  // backwards edges to -> from
1614  std::pair<edges_citerator, edges_citerator> bwd_edges_pair =
1615  this->m_graph->getEdges(to, from);
1616 
1617  // cout << "Backwards edges: " << endl;
1618  // for (edges_citerator e_it = bwd_edges_pair.first; e_it !=
1619  // bwd_edges_pair.second;
1620  //++e_it) {
1621  // cout << e_it->second << endl;
1622  //}
1623 
1624  for (edges_citerator edges_it = bwd_edges_pair.first;
1625  edges_it != bwd_edges_pair.second; ++edges_it)
1626  {
1627  // operate on a temporary object instead of the real edge - otherwise
1628  // function is non-const
1629  constraint_t curr_edge;
1630  (edges_it->second).inverse(curr_edge);
1631 
1632  // is it all 0s?
1633  CMatrixDouble33 inf_mat;
1634  curr_edge.getInformationMatrix(inf_mat);
1635 
1636  if (inf_mat == CMatrixDouble33() || std::isnan(inf_mat(0, 0)))
1637  {
1638  inf_mat.unit();
1639  curr_edge.cov_inv = inf_mat;
1640  }
1641 
1642  path_t curr_path(from); // set the starting node
1643  curr_path.addToPath(to, curr_edge);
1644 
1645  // update the resulting path_between_nodes if its determinant is smaller
1646  // than the determinant of the current path_between_nodes
1647  if (curr_determinant < curr_path.getDeterminant())
1648  {
1649  curr_determinant = curr_path.getDeterminant();
1650  *path_between_nodes = curr_path;
1651  }
1652  }
1653 
1654  MRPT_END;
1655 }
1656 
1657 template <class GRAPH_T>
1658 TUncertaintyPath<GRAPH_T>* CLoopCloserERD<GRAPH_T>::popMinUncertaintyPath(
1659  typename std::set<path_t*>* pool_of_paths) const
1660 {
1661  MRPT_START;
1662  using namespace std;
1663 
1664  // cout << "Determinants: ";
1665  path_t* optimal_path = NULL;
1666  double curr_determinant = 0;
1667  for (typename std::set<path_t*>::const_iterator it = pool_of_paths->begin();
1668  it != pool_of_paths->end(); ++it)
1669  {
1670  // cout << (*it)->getDeterminant() << ", ";
1671 
1672  // keep the largest determinant - we are in INFORMATION form.
1673  if (curr_determinant < (*it)->getDeterminant())
1674  {
1675  curr_determinant = (*it)->getDeterminant();
1676  optimal_path = *it;
1677  }
1678  }
1679 
1680  ASSERTDEB_(optimal_path);
1681  pool_of_paths->erase(optimal_path); // erase it from the pool
1682 
1683  return optimal_path;
1684  MRPT_END;
1685 }
1686 
1687 template <class GRAPH_T>
1688 bool CLoopCloserERD<GRAPH_T>::mahalanobisDistanceOdometryToICPEdge(
1689  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
1690  const constraint_t& rel_edge)
1691 {
1692  MRPT_START;
1693 
1694  using namespace std;
1695  using namespace mrpt::math;
1696 
1697  // mean difference
1698  pose_t initial_estim =
1699  this->m_graph->nodes.at(to) - this->m_graph->nodes.at(from);
1700  dynamic_vector<double> mean_diff;
1701  (rel_edge.getMeanVal() - initial_estim).getAsVector(mean_diff);
1702 
1703  // covariance matrix
1704  CMatrixDouble33 cov_mat;
1705  rel_edge.getCovariance(cov_mat);
1706 
1707  // mahalanobis distance computation
1708  double mahal_distance =
1709  mrpt::math::mahalanobisDistance2(mean_diff, cov_mat);
1710  bool mahal_distance_null = std::isnan(mahal_distance);
1711  if (!mahal_distance_null)
1712  {
1713  m_laser_params.mahal_distance_ICP_odom_win.addNewMeasurement(
1714  mahal_distance);
1715  }
1716 
1717  // double threshold = m_laser_params.mahal_distance_ICP_odom_win.getMean() +
1718  // 2*m_laser_params.mahal_distance_ICP_odom_win.getStdDev();
1719  double threshold =
1720  m_laser_params.mahal_distance_ICP_odom_win.getMedian() * 4;
1721  bool accept_edge =
1722  (threshold >= mahal_distance && !mahal_distance_null) ? true : false;
1723 
1724  // cout << "Suggested Edge: " << rel_edge.getMeanVal() << "|\tInitial
1725  // Estim.: " << initial_estim
1726  //<< "|\tMahalanobis Dist: " << mahal_distance << "|\tThresh.: " <<
1727  // threshold
1728  //<< " => " << (accept_edge? "ACCEPT": "REJECT") << endl;
1729 
1730  return accept_edge;
1731  MRPT_END;
1732 }
1733 
1734 template <class GRAPH_T>
1735 void CLoopCloserERD<GRAPH_T>::registerHypothesis(const hypot_t& hypot)
1736 {
1737  // MRPT_LOG_DEBUG_STREAM("Registering hypothesis: " <<
1738  // hypot.getAsString([>oneline=<] true));
1739  this->registerNewEdge(hypot.from, hypot.to, hypot.getEdge());
1740 }
1741 
1742 template <class GRAPH_T>
1743 void CLoopCloserERD<GRAPH_T>::registerNewEdge(
1744  const mrpt::graphs::TNodeID& from, const mrpt::graphs::TNodeID& to,
1745  const constraint_t& rel_edge)
1746 {
1747  MRPT_START;
1748  using namespace mrpt::math;
1749  using namespace std;
1750  parent_t::registerNewEdge(from, to, rel_edge);
1751 
1752  // keep track of the registered edges...
1753  m_edge_types_to_nums["ICP2D"]++;
1754 
1755  // keep track of the registered edges...
1756  if (absDiff(to, from) > m_lc_params.LC_min_nodeid_diff)
1757  {
1758  m_edge_types_to_nums["LC"]++;
1759  this->m_just_inserted_lc = true;
1760  MRPT_LOG_INFO("\tLoop Closure edge!");
1761  }
1762  else
1763  {
1764  this->m_just_inserted_lc = false;
1765  }
1766 
1767  // actuall registration
1768  this->m_graph->insertEdge(from, to, rel_edge);
1769 
1770  MRPT_END;
1771 }
1772 
1773 template <class GRAPH_T>
1774 void CLoopCloserERD<GRAPH_T>::setWindowManagerPtr(
1775  mrpt::graphslam::CWindowManager* win_manager)
1776 {
1777  // call parent_t class method
1778  parent_t::setWindowManagerPtr(win_manager);
1779 
1780  // may still be null..
1781  if (this->m_win_manager)
1782  {
1783  if (this->m_win_observer)
1784  {
1785  this->m_win_observer->registerKeystroke(
1786  m_laser_params.keystroke_laser_scans,
1787  "Toggle LaserScans Visualization");
1788  this->m_win_observer->registerKeystroke(
1789  m_lc_params.keystroke_map_partitions,
1790  "Toggle Map Partitions Visualization");
1791  }
1792 
1793  MRPT_LOG_DEBUG(
1794  "Fetched the window manager, window observer successfully.");
1795  }
1796 }
1797 template <class GRAPH_T>
1798 void CLoopCloserERD<GRAPH_T>::notifyOfWindowEvents(
1799  const std::map<std::string, bool>& events_occurred)
1800 {
1801  MRPT_START;
1802  parent_t::notifyOfWindowEvents(events_occurred);
1803 
1804  // laser scans
1805  if (events_occurred.at(m_laser_params.keystroke_laser_scans))
1806  {
1807  this->toggleLaserScansVisualization();
1808  }
1809  // map partitions
1810  if (events_occurred.at(m_lc_params.keystroke_map_partitions))
1811  {
1812  this->toggleMapPartitionsVisualization();
1813  }
1814 
1815  MRPT_END;
1816 }
1817 
1818 template <class GRAPH_T>
1819 void CLoopCloserERD<GRAPH_T>::initMapPartitionsVisualization()
1820 {
1821  using namespace mrpt;
1822  using namespace mrpt::gui;
1823  using namespace mrpt::math;
1824  using namespace mrpt::opengl;
1825 
1826  // textmessage - display the number of partitions
1827  this->m_win_manager->assignTextMessageParameters(
1828  /* offset_y* = */ &m_lc_params.offset_y_map_partitions,
1829  /* text_index* = */ &m_lc_params.text_index_map_partitions);
1830 
1831  // just add an empty CSetOfObjects in the scene - going to populate it later
1832  CSetOfObjects::Ptr map_partitions_obj =
1833  mrpt::make_aligned_shared<CSetOfObjects>();
1834  map_partitions_obj->setName("map_partitions");
1835 
1836  COpenGLScene::Ptr& scene = this->m_win->get3DSceneAndLock();
1837  scene->insert(map_partitions_obj);
1838  this->m_win->unlockAccess3DScene();
1839  this->m_win->forceRepaint();
1840 }
1841 
1842 template <class GRAPH_T>
1843 void CLoopCloserERD<GRAPH_T>::updateMapPartitionsVisualization()
1844 {
1845  using namespace mrpt;
1846  using namespace mrpt::gui;
1847  using namespace mrpt::math;
1848  using namespace mrpt::opengl;
1849  using namespace mrpt::poses;
1850 
1851  // textmessage
1852  std::stringstream title;
1853  title << "# Partitions: " << m_curr_partitions.size();
1854  this->m_win_manager->addTextMessage(
1855  5, -m_lc_params.offset_y_map_partitions, title.str(),
1856  mrpt::img::TColorf(m_lc_params.balloon_std_color),
1857  /* unique_index = */ m_lc_params.text_index_map_partitions);
1858 
1859  // update the partitioning visualization
1860  COpenGLScene::Ptr& scene = this->m_win->get3DSceneAndLock();
1861 
1862  // fetch the partitions CSetOfObjects
1863  CSetOfObjects::Ptr map_partitions_obj;
1864  {
1865  CRenderizable::Ptr obj = scene->getByName("map_partitions");
1866  // do not check for null ptr - must be properly created in the init*
1867  // method
1868  map_partitions_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
1869  }
1870 
1871  int partitionID = 0;
1872  bool partition_contains_last_node = false;
1873  bool found_last_node =
1874  false; // last node must exist in one partition at all cost TODO
1875  MRPT_LOG_DEBUG_STREAM(
1876  "Searching for the partition of the last nodeID: "
1877  << (this->m_graph->nodeCount() - 1));
1878 
1879  for (partitions_t::const_iterator p_it = m_curr_partitions.begin();
1880  p_it != m_curr_partitions.end(); ++p_it, ++partitionID)
1881  {
1882  // MRPT_LOG_DEBUG_STREAM("Working on Partition #" << partitionID);
1883  std::vector<uint32_t> nodes_list = *p_it;
1884 
1885  // finding the partition in which the last node is in
1886  if (std::find(
1887  nodes_list.begin(), nodes_list.end(),
1888  this->m_graph->nodeCount() - 1) != nodes_list.end())
1889  {
1890  partition_contains_last_node = true;
1891 
1892  found_last_node = true;
1893  }
1894  else
1895  {
1896  partition_contains_last_node = false;
1897  }
1898 
1899  // fetch the current partition object if it exists - create otherwise
1900  std::string partition_obj_name =
1901  mrpt::format("partition_%d", partitionID);
1902  std::string balloon_obj_name = mrpt::format("#%d", partitionID);
1903 
1904  CRenderizable::Ptr obj =
1905  map_partitions_obj->getByName(partition_obj_name);
1906  CSetOfObjects::Ptr curr_partition_obj;
1907  if (obj)
1908  {
1909  // MRPT_LOG_DEBUG_STREAM(
1910  //"\tFetching CSetOfObjects partition object for partition #" <<
1911  // partitionID);
1912  curr_partition_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
1913  if (m_lc_params.LC_check_curr_partition_only)
1914  { // make all but the last partition invisible
1915  curr_partition_obj->setVisibility(partition_contains_last_node);
1916  }
1917  }
1918  else
1919  {
1920  MRPT_LOG_DEBUG_STREAM(
1921  "\tCreating a new CSetOfObjects partition object for partition "
1922  "#"
1923  << partitionID);
1924  curr_partition_obj = mrpt::make_aligned_shared<CSetOfObjects>();
1925  curr_partition_obj->setName(partition_obj_name);
1926  if (m_lc_params.LC_check_curr_partition_only)
1927  {
1928  // make all but the last partition invisible
1929  curr_partition_obj->setVisibility(partition_contains_last_node);
1930  }
1931 
1932  // MRPT_LOG_DEBUG_STREAM("\t\tCreating a new CSphere balloon
1933  // object");
1934  CSphere::Ptr balloon_obj = mrpt::make_aligned_shared<CSphere>();
1935  balloon_obj->setName(balloon_obj_name);
1936  balloon_obj->setRadius(m_lc_params.balloon_radius);
1937  balloon_obj->setColor_u8(m_lc_params.balloon_std_color);
1938  balloon_obj->enableShowName();
1939 
1940  curr_partition_obj->insert(balloon_obj);
1941 
1942  // set of lines connecting the graph nodes to the balloon
1943  // MRPT_LOG_DEBUG_STREAM(
1944  //"\t\tCreating set of lines that will connect to the Balloon");
1945  CSetOfLines::Ptr connecting_lines_obj =
1946  mrpt::make_aligned_shared<CSetOfLines>();
1947  connecting_lines_obj->setName("connecting_lines");
1948  connecting_lines_obj->setColor_u8(
1949  m_lc_params.connecting_lines_color);
1950  connecting_lines_obj->setLineWidth(0.1f);
1951 
1952  curr_partition_obj->insert(connecting_lines_obj);
1953 
1954  // add the created CSetOfObjects to the total CSetOfObjects
1955  // responsible
1956  // for the map partitioning
1957  map_partitions_obj->insert(curr_partition_obj);
1958  // MRPT_LOG_DEBUG_STREAM("\tInserted new CSetOfObjects
1959  // successfully");
1960  }
1961  // up to now the CSetOfObjects exists and the balloon inside it as
1962  // well..
1963 
1964  std::pair<double, double> centroid_coords;
1965  this->computeCentroidOfNodesVector(nodes_list, &centroid_coords);
1966 
1967  TPoint3D balloon_location(
1968  centroid_coords.first, centroid_coords.second,
1969  m_lc_params.balloon_elevation);
1970 
1971  // MRPT_LOG_DEBUG_STREAM("\tUpdating the balloon position");
1972  // set the balloon properties
1973  CSphere::Ptr balloon_obj;
1974  {
1975  // place the partitions baloon at the centroid elevated by a fixed Z
1976  // value
1977  CRenderizable::Ptr obj =
1978  curr_partition_obj->getByName(balloon_obj_name);
1979  balloon_obj = std::dynamic_pointer_cast<CSphere>(obj);
1980  balloon_obj->setLocation(balloon_location);
1981  if (partition_contains_last_node)
1982  balloon_obj->setColor_u8(m_lc_params.balloon_curr_color);
1983  else
1984  balloon_obj->setColor_u8(m_lc_params.balloon_std_color);
1985  }
1986 
1987  // MRPT_LOG_DEBUG_STREAM("\tUpdating the lines connecting nodes to
1988  // balloon");
1989  // set the lines connecting the nodes of the partition to the partition
1990  // balloon - set it from scratch all the times since the node positions
1991  // tend to change according to the dijkstra position estimation
1992  CSetOfLines::Ptr connecting_lines_obj;
1993  {
1994  // place the partitions baloon at the centroid elevated by a fixed Z
1995  // value
1996  CRenderizable::Ptr obj =
1997  curr_partition_obj->getByName("connecting_lines");
1998  connecting_lines_obj = std::dynamic_pointer_cast<CSetOfLines>(obj);
1999 
2000  connecting_lines_obj->clear();
2001 
2002  for (std::vector<uint32_t>::const_iterator it = nodes_list.begin();
2003  it != nodes_list.end(); ++it)
2004  {
2005  CPose3D curr_pose(this->m_graph->nodes.at(*it));
2006  TPoint3D curr_node_location(curr_pose.asTPose());
2007 
2008  TSegment3D connecting_line(
2009  curr_node_location, balloon_location);
2010  connecting_lines_obj->appendLine(connecting_line);
2011  }
2012  }
2013  // MRPT_LOG_DEBUG_STREAM("Done working on partition #" << partitionID);
2014  }
2015 
2016  if (!found_last_node)
2017  {
2018  /// @todo Have some sorts of a string_view instead
2019  THROW_EXCEPTION("Last inserted nodeID was not found in any partition.");
2020  }
2021 
2022  // remove outdated partitions
2023  // these occur when more partitions existed during the previous
2024  // visualization
2025  // update, thus the partitions with higher ID than the maximum partitionID
2026  // would otherwise remain in the visual as zombie partitions
2027  size_t prev_size = m_last_partitions.size();
2028  size_t curr_size = m_curr_partitions.size();
2029  if (curr_size < prev_size)
2030  {
2031  MRPT_LOG_DEBUG_STREAM("Removing outdated partitions in visual");
2032  for (size_t partitionID = curr_size; partitionID != prev_size;
2033  ++partitionID)
2034  {
2035  MRPT_LOG_DEBUG_STREAM("\tRemoving partition " << partitionID);
2036  std::string partition_obj_name = mrpt::format(
2037  "partition_%lu", static_cast<unsigned long>(partitionID));
2038 
2039  CRenderizable::Ptr obj =
2040  map_partitions_obj->getByName(partition_obj_name);
2041  if (!obj)
2042  {
2043  THROW_EXCEPTION_FMT("Partition: %s was not found",
2044  partition_obj_name.c_str())
2045  }
2046  map_partitions_obj->removeObject(obj);
2047  }
2048  }
2049  MRPT_LOG_DEBUG_STREAM("Done working on the partitions visualization.");
2050 
2051  this->m_win->unlockAccess3DScene();
2052  this->m_win->forceRepaint();
2053 } // end of updateMapPartitionsVisualization
2054 
2055 template <class GRAPH_T>
2056 void CLoopCloserERD<GRAPH_T>::toggleMapPartitionsVisualization()
2057 {
2058  MRPT_START;
2059  ASSERTDEBMSG_(this->m_win, "No CDisplayWindow3D* was provided");
2060  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* was provided");
2061  using namespace mrpt::opengl;
2062 
2063  MRPT_LOG_INFO("Toggling map partitions visualization...");
2064  mrpt::opengl::COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
2065 
2066  if (m_lc_params.visualize_map_partitions)
2067  {
2068  mrpt::opengl::CRenderizable::Ptr obj =
2069  scene->getByName("map_partitions");
2070  obj->setVisibility(!obj->isVisible());
2071  }
2072  else
2073  {
2074  this->dumpVisibilityErrorMsg("visualize_map_partitions");
2075  }
2076 
2077  this->m_win->unlockAccess3DScene();
2078  this->m_win->forceRepaint();
2079 
2080  MRPT_END;
2081 } // end of toggleMapPartitionsVisualization
2082 
2083 template <class GRAPH_T>
2084 void CLoopCloserERD<GRAPH_T>::computeCentroidOfNodesVector(
2085  const std::vector<uint32_t>& nodes_list,
2086  std::pair<double, double>* centroid_coords) const
2087 {
2088  MRPT_START;
2089 
2090  // get the poses and find the centroid so that we can place the baloon over
2091  // and at their center
2092  double centroid_x = 0;
2093  double centroid_y = 0;
2094  for (std::vector<uint32_t>::const_iterator node_it = nodes_list.begin();
2095  node_it != nodes_list.end(); ++node_it)
2096  {
2097  pose_t curr_node_pos = this->m_graph->nodes.at(*node_it);
2098  centroid_x += curr_node_pos.x();
2099  centroid_y += curr_node_pos.y();
2100  }
2101 
2102  // normalize by the size - assign to the given pair
2103  centroid_coords->first =
2104  centroid_x / static_cast<double>(nodes_list.size());
2105  centroid_coords->second =
2106  centroid_y / static_cast<double>(nodes_list.size());
2107 
2108  MRPT_END;
2109 } // end of computeCentroidOfNodesVector
2110 
2111 template <class GRAPH_T>
2112 void CLoopCloserERD<GRAPH_T>::initLaserScansVisualization()
2113 {
2114  MRPT_START;
2115 
2116  // laser scan visualization
2117  if (m_laser_params.visualize_laser_scans)
2118  {
2119  mrpt::opengl::COpenGLScene::Ptr scene =
2120  this->m_win->get3DSceneAndLock();
2121 
2122  mrpt::opengl::CPlanarLaserScan::Ptr laser_scan_viz =
2123  mrpt::make_aligned_shared<mrpt::opengl::CPlanarLaserScan>();
2124  laser_scan_viz->enablePoints(true);
2125  laser_scan_viz->enableLine(true);
2126  laser_scan_viz->enableSurface(true);
2127  laser_scan_viz->setSurfaceColor(
2128  m_laser_params.laser_scans_color.R,
2129  m_laser_params.laser_scans_color.G,
2130  m_laser_params.laser_scans_color.B,
2131  m_laser_params.laser_scans_color.A);
2132 
2133  laser_scan_viz->setName("laser_scan_viz");
2134 
2135  scene->insert(laser_scan_viz);
2136  this->m_win->unlockAccess3DScene();
2137  this->m_win->forceRepaint();
2138  }
2139 
2140  MRPT_END;
2141 }
2142 
2143 template <class GRAPH_T>
2144 void CLoopCloserERD<GRAPH_T>::updateLaserScansVisualization()
2145 {
2146  MRPT_START;
2147 
2148  // update laser scan visual
2149  if (m_laser_params.visualize_laser_scans && m_last_laser_scan2D)
2150  {
2151  mrpt::opengl::COpenGLScene::Ptr scene =
2152  this->m_win->get3DSceneAndLock();
2153 
2154  mrpt::opengl::CRenderizable::Ptr obj =
2155  scene->getByName("laser_scan_viz");
2156  mrpt::opengl::CPlanarLaserScan::Ptr laser_scan_viz =
2157  std::dynamic_pointer_cast<mrpt::opengl::CPlanarLaserScan>(obj);
2158  laser_scan_viz->setScan(*m_last_laser_scan2D);
2159 
2160  // set the pose of the laser scan
2161  const auto search =
2162  this->m_graph->nodes.find(this->m_graph->nodeCount() - 1);
2163  if (search != this->m_graph->nodes.end())
2164  {
2165  laser_scan_viz->setPose(search->second);
2166  // put the laser scan underneath the graph, so that you can still
2167  // visualize the loop closures with the nodes ahead
2168  laser_scan_viz->setPose(
2169  mrpt::poses::CPose3D(
2170  laser_scan_viz->getPoseX(), laser_scan_viz->getPoseY(),
2171  -0.15, mrpt::DEG2RAD(laser_scan_viz->getPoseYaw()),
2172  mrpt::DEG2RAD(laser_scan_viz->getPosePitch()),
2173  mrpt::DEG2RAD(laser_scan_viz->getPoseRoll())));
2174  }
2175 
2176  this->m_win->unlockAccess3DScene();
2177  this->m_win->forceRepaint();
2178  }
2179 
2180  MRPT_END;
2181 }
2182 
2183 template <class GRAPH_T>
2184 void CLoopCloserERD<GRAPH_T>::toggleLaserScansVisualization()
2185 {
2186  MRPT_START;
2187  ASSERTDEBMSG_(this->m_win, "No CDisplayWindow3D* was provided");
2188  ASSERTDEBMSG_(this->m_win_manager, "No CWindowManager* was provided");
2189 
2190  MRPT_LOG_INFO("Toggling LaserScans visualization...");
2191 
2192  mrpt::opengl::COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
2193 
2194  if (m_laser_params.visualize_laser_scans)
2195  {
2196  mrpt::opengl::CRenderizable::Ptr obj =
2197  scene->getByName("laser_scan_viz");
2198  obj->setVisibility(!obj->isVisible());
2199  }
2200  else
2201  {
2202  this->dumpVisibilityErrorMsg("visualize_laser_scans");
2203  }
2204 
2205  this->m_win->unlockAccess3DScene();
2206  this->m_win->forceRepaint();
2207 
2208  MRPT_END;
2209 }
2210 
2211 template <class GRAPH_T>
2212 void CLoopCloserERD<GRAPH_T>::getEdgesStats(
2213  std::map<std::string, int>* edge_types_to_num) const
2214 {
2215  MRPT_START;
2216  *edge_types_to_num = m_edge_types_to_nums;
2217  MRPT_END;
2218 }
2219 
2220 template <class GRAPH_T>
2221 void CLoopCloserERD<GRAPH_T>::initializeVisuals()
2222 {
2223  MRPT_START;
2224  parent_t::initializeVisuals();
2225  // MRPT_LOG_DEBUG_STREAM("Initializing visuals");
2226  this->m_time_logger.enter("Visuals");
2227 
2228  ASSERTDEBMSG_(
2229  m_laser_params.has_read_config,
2230  "Configuration parameters aren't loaded yet");
2231  if (m_laser_params.visualize_laser_scans)
2232  {
2233  this->initLaserScansVisualization();
2234  }
2235  if (m_lc_params.visualize_map_partitions)
2236  {
2237  this->initMapPartitionsVisualization();
2238  }
2239 
2240  if (m_visualize_curr_node_covariance)
2241  {
2242  this->initCurrCovarianceVisualization();
2243  }
2244 
2245  this->m_time_logger.leave("Visuals");
2246  MRPT_END;
2247 }
2248 template <class GRAPH_T>
2249 void CLoopCloserERD<GRAPH_T>::updateVisuals()
2250 {
2251  MRPT_START;
2252  parent_t::updateVisuals();
2253  // MRPT_LOG_DEBUG_STREAM("Updating visuals");
2254  this->m_time_logger.enter("Visuals");
2255 
2256  if (m_laser_params.visualize_laser_scans)
2257  {
2258  this->updateLaserScansVisualization();
2259  }
2260  if (m_lc_params.visualize_map_partitions)
2261  {
2262  this->updateMapPartitionsVisualization();
2263  }
2264  if (m_visualize_curr_node_covariance)
2265  {
2266  this->updateCurrCovarianceVisualization();
2267  }
2268 
2269  this->m_time_logger.leave("Visuals");
2270  MRPT_END;
2271 }
2272 
2273 template <class GRAPH_T>
2274 void CLoopCloserERD<GRAPH_T>::initCurrCovarianceVisualization()
2275 {
2276  MRPT_START;
2277  using namespace std;
2278  using namespace mrpt::opengl;
2279 
2280  // text message for covariance ellipsis
2281  this->m_win_manager->assignTextMessageParameters(
2282  /* offset_y* = */ &m_offset_y_curr_node_covariance,
2283  /* text_index* = */ &m_text_index_curr_node_covariance);
2284 
2285  std::string title("Position uncertainty");
2286  this->m_win_manager->addTextMessage(
2287  5, -m_offset_y_curr_node_covariance, title,
2288  mrpt::img::TColorf(m_curr_node_covariance_color),
2289  /* unique_index = */ m_text_index_curr_node_covariance);
2290 
2291  // covariance ellipsis
2292  CEllipsoid::Ptr cov_ellipsis_obj = mrpt::make_aligned_shared<CEllipsoid>();
2293  cov_ellipsis_obj->setName("cov_ellipsis_obj");
2294  cov_ellipsis_obj->setColor_u8(m_curr_node_covariance_color);
2295  cov_ellipsis_obj->setLocation(0, 0, 0);
2296  // cov_ellipsis_obj->setQuantiles(2.0);
2297 
2298  mrpt::opengl::COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
2299  scene->insert(cov_ellipsis_obj);
2300  this->m_win->unlockAccess3DScene();
2301  this->m_win->forceRepaint();
2302 
2303  MRPT_END;
2304 }
2305 
2306 template <class GRAPH_T>
2307 void CLoopCloserERD<GRAPH_T>::updateCurrCovarianceVisualization()
2308 {
2309  MRPT_START;
2310  using namespace std;
2311  using namespace mrpt::math;
2312  using namespace mrpt::opengl;
2313  using namespace mrpt::gui;
2314 
2315  // get the optimal path to the current node
2316  mrpt::graphs::TNodeID curr_node = this->m_graph->nodeCount() - 1;
2317  path_t* path = queryOptimalPath(curr_node);
2318  if (!path) return;
2319 
2320  CMatrixDouble33 mat;
2321  path->curr_pose_pdf.getCovariance(mat);
2322  pose_t curr_position = this->m_graph->nodes.at(curr_node);
2323 
2324  MRPT_LOG_DEBUG_STREAM(
2325  "In updateCurrCovarianceVisualization\n"
2326  "Covariance matrix:\n"
2327  << mat << "\n"
2328  "determinant : "
2329  << mat.det());
2330 
2331  mrpt::opengl::COpenGLScene::Ptr scene = this->m_win->get3DSceneAndLock();
2332  CRenderizable::Ptr obj = scene->getByName("cov_ellipsis_obj");
2333  CEllipsoid::Ptr cov_ellipsis_obj =
2334  std::dynamic_pointer_cast<CEllipsoid>(obj);
2335 
2336  // set the pose and corresponding covariance matrix of the ellipsis
2337  cov_ellipsis_obj->setLocation(curr_position.x(), curr_position.y(), 0);
2338  // pose_t loc = path->curr_pose_pdf.getMeanVal();
2339  // cov_ellipsis_obj->setLocation(loc.x(), loc.y(), 0);
2340  cov_ellipsis_obj->setCovMatrix(mat, 2);
2341 
2342  this->m_win->unlockAccess3DScene();
2343  this->m_win->forceRepaint();
2344 
2345  MRPT_END;
2346 }
2347 
2348 template <class GRAPH_T>
2349 void CLoopCloserERD<GRAPH_T>::dumpVisibilityErrorMsg(
2350  std::string viz_flag, int sleep_time /* = 500 milliseconds */)
2351 {
2352  MRPT_START;
2353 
2354  this->logFmt(
2355  mrpt::system::LVL_ERROR,
2356  "Cannot toggle visibility of specified object.\n "
2357  "Make sure that the corresponding visualization flag ( %s "
2358  ") is set to true in the .ini file.\n",
2359  viz_flag.c_str());
2360  std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));
2361 
2362  MRPT_END;
2363 }
2364 
2365 template <class GRAPH_T>
2366 void CLoopCloserERD<GRAPH_T>::loadParams(const std::string& source_fname)
2367 {
2368  MRPT_START;
2369  parent_t::loadParams(source_fname);
2370 
2371  m_partitioner.options.loadFromConfigFileName(
2372  source_fname, "EdgeRegistrationDeciderParameters");
2373  m_laser_params.loadFromConfigFileName(
2374  source_fname, "EdgeRegistrationDeciderParameters");
2375  m_lc_params.loadFromConfigFileName(
2376  source_fname, "EdgeRegistrationDeciderParameters");
2377 
2378  mrpt::config::CConfigFile source(source_fname);
2379 
2380  m_consec_icp_constraint_factor = source.read_double(
2381  "EdgeRegistrationDeciderParameters", "consec_icp_constraint_factor",
2382  0.90, false);
2383  m_lc_icp_constraint_factor = source.read_double(
2384  "EdgeRegistrationDeciderParameters", "lc_icp_constraint_factor", 0.70,
2385  false);
2386 
2387  // set the logging level if given by the user
2388  int min_verbosity_level = source.read_int(
2389  "EdgeRegistrationDeciderParameters", "class_verbosity", 1, false);
2390 
2391  this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));
2392  MRPT_END;
2393 }
2394 template <class GRAPH_T>
2395 void CLoopCloserERD<GRAPH_T>::printParams() const
2396 {
2397  MRPT_START;
2398  using namespace std;
2399 
2400  cout << "------------------[Pair-wise Consistency of ICP Edges - "
2401  "Registration Procedure Summary]------------------"
2402  << endl;
2403 
2404  parent_t::printParams();
2405  m_partitioner.options.dumpToConsole();
2406  m_laser_params.dumpToConsole();
2407  m_lc_params.dumpToConsole();
2408 
2409  cout << "Scan-matching ICP Constraint factor: "
2410  << m_consec_icp_constraint_factor << endl;
2411  cout << "Loop-closure ICP Constraint factor: "
2412  << m_lc_icp_constraint_factor << endl;
2413 
2414  MRPT_LOG_DEBUG_STREAM("Printed the relevant parameters");
2415  MRPT_END;
2416 }
2417 
2418 template <class GRAPH_T>
2419 void CLoopCloserERD<GRAPH_T>::getDescriptiveReport(
2420  std::string* report_str) const
2421 {
2422  MRPT_START;
2423 
2424  // Report on graph
2425  std::stringstream class_props_ss;
2426  class_props_ss << "Pair-wise Consistency of ICP Edges - Registration "
2427  "Procedure Summary: "
2428  << std::endl;
2429  class_props_ss << this->header_sep << std::endl;
2430 
2431  // time and output logging
2432  const std::string time_res = this->m_time_logger.getStatsAsText();
2433  const std::string output_res = this->getLogAsString();
2434 
2435  // merge the individual reports
2436  report_str->clear();
2437  parent_t::getDescriptiveReport(report_str);
2438 
2439  *report_str += class_props_ss.str();
2440  *report_str += this->report_sep;
2441 
2442  *report_str += time_res;
2443  *report_str += this->report_sep;
2444 
2445  *report_str += output_res;
2446  *report_str += this->report_sep;
2447 
2448  MRPT_END;
2449 } // end of getDescriptiveReport
2450 
2451 template <class GRAPH_T>
2452 void CLoopCloserERD<GRAPH_T>::getCurrentPartitions(
2453  partitions_t& partitions_out) const
2454 {
2455  partitions_out = this->getCurrentPartitions();
2456 }
2457 
2458 template <class GRAPH_T>
2459 const std::vector<std::vector<uint32_t>>&
2460  CLoopCloserERD<GRAPH_T>::getCurrentPartitions() const
2461 {
2462  return m_curr_partitions;
2463 }
2464 
2465 template <class GRAPH_T>
2466 void CLoopCloserERD<GRAPH_T>::updateMapPartitions(
2467  bool full_update /* = false */, bool is_first_time_node_reg /* = false */)
2468 {
2469  MRPT_START;
2470  using namespace mrpt::math;
2471  using namespace std;
2472  this->m_time_logger.enter("updateMapPartitions");
2473 
2474  // Initialize the nodeIDs => LaserScans map
2475  nodes_to_scans2D_t nodes_to_scans;
2476  if (full_update)
2477  {
2478  MRPT_LOG_INFO(
2479  "updateMapPartitions: Full partitioning of map was issued");
2480  // clear the existing partitions and recompute the partitioned map for
2481  // all the nodes
2482  m_partitioner.clear();
2483  nodes_to_scans = this->m_nodes_to_laser_scans2D;
2484  }
2485  else
2486  {
2487  // if registering measurement for root node as well...
2488  if (is_first_time_node_reg)
2489  {
2490  nodes_to_scans.insert(
2491  make_pair(
2492  this->m_graph->root,
2493  this->m_nodes_to_laser_scans2D.at(this->m_graph->root)));
2494  }
2495 
2496  // just use the last node-laser scan pair
2497  nodes_to_scans.insert(
2498  make_pair(
2499  this->m_graph->nodeCount() - 1,
2500  this->m_nodes_to_laser_scans2D.at(
2501  this->m_graph->nodeCount() - 1)));
2502  }
2503 
2504  // TODO - Should always exist.
2505  // for each one of the above nodes - add its position and correspoding
2506  // laserScan to the partitioner object
2507  for (auto it = nodes_to_scans.begin(); it != nodes_to_scans.end(); ++it)
2508  {
2509  if (!it->second)
2510  { // if laserScan invalid go to next...
2511  MRPT_LOG_WARN_STREAM(
2512  "nodeID \"" << it->first << "\" has invalid laserScan");
2513  continue;
2514  }
2515 
2516  // find pose of node, if it exists...
2517  // TODO - investigate this case. Why should this be happening?
2518  const auto& search = this->m_graph->nodes.find(it->first);
2519  if (search == this->m_graph->nodes.end())
2520  {
2521  MRPT_LOG_WARN_STREAM("Couldn't find pose for nodeID " << it->first);
2522  continue;
2523  }
2524 
2525  // pose
2526  const auto curr_constraint = constraint_t(search->second);
2527  const auto pose3d(mrpt::poses::CPose3DPDF::createFrom2D(curr_constraint));
2528 
2529  // laser scan
2530  mrpt::obs::CSensoryFrame sf;
2531  sf.insert(it->second);
2532 
2533  m_partitioner.addMapFrame(sf, *pose3d);
2534  }
2535 
2536  // update the last partitions list
2537  size_t curr_size = m_curr_partitions.size();
2538  m_last_partitions.resize(curr_size);
2539  for (size_t i = 0; i < curr_size; i++)
2540  {
2541  m_last_partitions[i] = m_curr_partitions[i];
2542  }
2543  // update current partitions list
2544  m_partitioner.updatePartitions(m_curr_partitions);
2545 
2546  MRPT_LOG_DEBUG_STREAM("Updated map partitions successfully.");
2547  this->m_time_logger.leave("updateMapPartitions");
2548  MRPT_END;
2549 } // end of updateMapPartitions
2550 
2551 // TLaserParams
2552 // //////////////////////////////////
2553 
2554 template <class GRAPH_T>
2555 CLoopCloserERD<GRAPH_T>::TLaserParams::TLaserParams()
2556  : laser_scans_color(0, 20, 255),
2557  keystroke_laser_scans("l"),
2558  has_read_config(false)
2559 {
2560  mahal_distance_ICP_odom_win.resizeWindow(
2561  200); // use the last X mahalanobis distance values
2562  goodness_threshold_win.resizeWindow(200); // use the last X ICP values
2563 }
2564 
2565 template <class GRAPH_T>
2566 CLoopCloserERD<GRAPH_T>::TLaserParams::~TLaserParams()
2567 {
2568 }
2569 
2570 template <class GRAPH_T>
2571 void CLoopCloserERD<GRAPH_T>::TLaserParams::dumpToTextStream(
2572  std::ostream& out) const
2573 {
2574  MRPT_START;
2575 
2576  out << "Use scan-matching constraints = "
2577  << (use_scan_matching ? "TRUE" : "FALSE") << std::endl;
2578  out << "Num. of previous nodes to check ICP against = "
2579  << prev_nodes_for_ICP << std::endl;
2580  out << "Visualize laser scans = "
2581  << (visualize_laser_scans ? "TRUE" : "FALSE") << std::endl;
2582 
2583  MRPT_END;
2584 }
2585 template <class GRAPH_T>
2586 void CLoopCloserERD<GRAPH_T>::TLaserParams::loadFromConfigFile(
2587  const mrpt::config::CConfigFileBase& source, const std::string& section)
2588 {
2589  MRPT_START;
2590 
2591  use_scan_matching =
2592  source.read_bool(section, "use_scan_matching", true, false);
2593  prev_nodes_for_ICP = source.read_int( // how many nodes to check ICP against
2594  section,
2595  "prev_nodes_for_ICP",
2596  10, false);
2597  visualize_laser_scans = source.read_bool(
2598  "VisualizationParameters", "visualize_laser_scans", true, false);
2599 
2600  has_read_config = true;
2601  MRPT_END;
2602 }
2603 // TLoopClosureParams
2604 // //////////////////////////////////
2605 
2606 template <class GRAPH_T>
2607 CLoopCloserERD<GRAPH_T>::TLoopClosureParams::TLoopClosureParams()
2608  : keystroke_map_partitions("b"),
2609  balloon_elevation(3),
2610  balloon_radius(0.5),
2611  balloon_std_color(153, 0, 153),
2612  balloon_curr_color(62, 0, 80),
2613  connecting_lines_color(balloon_std_color),
2614  has_read_config(false)
2615 {
2616 }
2617 
2618 template <class GRAPH_T>
2619 CLoopCloserERD<GRAPH_T>::TLoopClosureParams::~TLoopClosureParams()
2620 {
2621 }
2622 
2623 template <class GRAPH_T>
2624 void CLoopCloserERD<GRAPH_T>::TLoopClosureParams::dumpToTextStream(
2625  std::ostream& out) const
2626 {
2627  MRPT_START;
2628  using namespace std;
2629 
2630  stringstream ss;
2631  ss << "Min. node difference for loop closure = "
2632  << LC_min_nodeid_diff << endl;
2633  ss << "Remote NodeIDs to consider the potential loop closure = "
2634  << LC_min_remote_nodes << endl;
2635  ss << "Min EigenValues ratio for accepting a hypotheses set = "
2636  << LC_eigenvalues_ratio_thresh << endl;
2637  ss << "Check only current node's partition for loop closures = "
2638  << (LC_check_curr_partition_only ? "TRUE" : "FALSE") << endl;
2639  ss << "New registered nodes required for full partitioning = "
2640  << full_partition_per_nodes << endl;
2641  ss << "Visualize map partitions = "
2642  << (visualize_map_partitions ? "TRUE" : "FALSE") << endl;
2643 
2644  out << mrpt::format("%s", ss.str().c_str());
2645 
2646  MRPT_END;
2647 }
2648 template <class GRAPH_T>
2649 void CLoopCloserERD<GRAPH_T>::TLoopClosureParams::loadFromConfigFile(
2650  const mrpt::config::CConfigFileBase& source, const std::string& section)
2651 {
2652  MRPT_START;
2653  LC_min_nodeid_diff = source.read_int(
2654  "GeneralConfiguration", "LC_min_nodeid_diff", 30, false);
2655  LC_min_remote_nodes =
2656  source.read_int(section, "LC_min_remote_nodes", 3, false);
2657  LC_eigenvalues_ratio_thresh =
2658  source.read_double(section, "LC_eigenvalues_ratio_thresh", 2, false);
2659  LC_check_curr_partition_only =
2660  source.read_bool(section, "LC_check_curr_partition_only", true, false);
2661  full_partition_per_nodes =
2662  source.read_int(section, "full_partition_per_nodes", 50, false);
2663  visualize_map_partitions = source.read_bool(
2664  "VisualizationParameters", "visualize_map_partitions", true, false);
2665 
2666  has_read_config = true;
2667  MRPT_END;
2668 }
2669 }
2670 }
2671 } // end of namespaces
2672 
2673 #endif /* end of include guard: CLOOPCLOSERERD_IMPL_H */
mrpt::obs::CObservation::Ptr
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
mrpt::opengl::CSetOfLines::Ptr
std::shared_ptr< CSetOfLines > Ptr
Definition: CSetOfLines.h:35
mrpt::obs::gnss::a1
double a1
Definition: gnss_messages_novatel.h:454
format
GLenum GLsizei GLenum format
Definition: glext.h:3531
mrpt::slam::CICP::TReturnInfo::goodness
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
Definition: CICP.h:199
mrpt::graphslam::deciders::CLoopCloserERD::splitPartitionToGroups
void splitPartitionToGroups(std::vector< uint32_t > &partition, std::vector< uint32_t > *groupA, std::vector< uint32_t > *groupB, int max_nodes_in_group=5)
Split an existing partition to Groups.
Definition: CLoopCloserERD_impl.h:683
mrpt::graphslam::deciders::CLoopCloserERD::evalPWConsistenciesMatrix
void evalPWConsistenciesMatrix(const mrpt::math::CMatrixDouble &consist_matrix, const hypotsp_t &hypots_pool, hypotsp_t *valid_hypots)
Evalute the consistencies matrix, fill the valid hypotheses.
Definition: CLoopCloserERD_impl.h:609
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:27
mrpt::graphslam::deciders::CLoopCloserERD::updateMapPartitionsVisualization
void updateMapPartitionsVisualization()
Update the map partitions visualization.
Definition: CLoopCloserERD_impl.h:1843
mrpt::math::mahalanobisDistance2
MAT::Scalar mahalanobisDistance2(const VECTORLIKE1 &X, const VECTORLIKE2 &MU, const MAT &COV)
Computes the squared mahalanobis distance of a vector X given the mean MU and the covariance inverse ...
Definition: data_utils.h:34
mrpt::graphslam::deciders::CLoopCloserERD::TLoopClosureParams::TLoopClosureParams
TLoopClosureParams()
Definition: CLoopCloserERD_impl.h:2607
MRPT_LOG_INFO
#define MRPT_LOG_INFO(_STRING)
Definition: system/COutputLogger.h:429
mrpt::obs::CSensoryFrame::Ptr
std::shared_ptr< CSensoryFrame > Ptr
Definition: CSensoryFrame.h:56
mrpt::graphslam::deciders::CLoopCloserERD::TLoopClosureParams::~TLoopClosureParams
~TLoopClosureParams()
Definition: CLoopCloserERD_impl.h:2619
mrpt::graphslam::detail::TNodeProps::pose
GRAPH_T::global_pose_t pose
Definition: TNodeProps.h:24
mrpt::graphslam::deciders::CLoopCloserERD::TGenerateHypotsPoolAdParams
Struct for passing additional parameters to the generateHypotsPool call.
Definition: CLoopCloserERD.h:326
mrpt::graphslam::deciders::CLoopCloserERD::TLoopClosureParams::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CLoopCloserERD_impl.h:2649
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
src
GLuint src
Definition: glext.h:7278
CSphere.h
mrpt::containers::getSTLContainerAsString
std::string getSTLContainerAsString(const T &t)
Return a STL container in std::string form.
Definition: stl_containers_utils.h:80
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Definition: eigen_frwds.h:44
mrpt::opengl::CSphere::Ptr
std::shared_ptr< CSphere > Ptr
Definition: CSphere.h:33
mrpt::graphslam::deciders::CLoopCloserERD::execDijkstraProjection
void execDijkstraProjection(mrpt::graphs::TNodeID starting_node=0, mrpt::graphs::TNodeID ending_node=INVALID_NODEID)
compute the minimum uncertainty of each node position with regards to the graph root.
Definition: CLoopCloserERD_impl.h:1340
mrpt::graphslam::TUncertaintyPath::clear
void clear()
Definition: TUncertaintyPath_impl.h:49
mrpt::graphslam::deciders::CLoopCloserERD::addScanMatchingEdges
virtual void addScanMatchingEdges(const mrpt::graphs::TNodeID &curr_nodeID)
Addd ICP constraints from X previous nodeIDs up to the given nodeID.
Definition: CLoopCloserERD_impl.h:191
mrpt::graphslam::deciders::CLoopCloserERD::TLaserParams::TLaserParams
TLaserParams()
Definition: CLoopCloserERD_impl.h:2555
obj
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
mrpt::graphs::TNodeID
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:17
mrpt::graphslam::deciders::CLoopCloserERD::initMapPartitionsVisualization
void initMapPartitionsVisualization()
Initialize the visualization of the map partition objects.
Definition: CLoopCloserERD_impl.h:1819
mrpt::graphslam::deciders::CLoopCloserERD::updateCurrCovarianceVisualization
void updateCurrCovarianceVisualization()
Definition: CLoopCloserERD_impl.h:2307
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
mrpt::graphslam::deciders::CLoopCloserERD::TLaserParams::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section)
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CLoopCloserERD_impl.h:2586
mrpt::graphslam::deciders::CLoopCloserERD::getEdgesStats
void getEdgesStats(std::map< std::string, int > *edge_types_to_num) const
Definition: CLoopCloserERD_impl.h:2212
mrpt::graphs::detail::THypothesis
An edge hypothesis between two nodeIDs.
Definition: THypothesis.h:37
mrpt::containers::find
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:219
data_utils.h
mrpt::math::absDiff
T absDiff(const T &lhs, const T &rhs)
Absolute difference between two numbers.
Definition: math/include/mrpt/math/utils.h:54
mrpt::graphslam::deciders::CLoopCloserERD::updateLaserScansVisualization
void updateLaserScansVisualization()
Definition: CLoopCloserERD_impl.h:2144
mrpt::math::CMatrixDouble33
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:57
mrpt::graphs::HypothesisNotFoundException
Definition: CHypothesisNotFoundException.h:22
mrpt::opengl::CRenderizable::Ptr
std::shared_ptr< CRenderizable > Ptr
Definition: CRenderizable.h:45
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:30
CEllipsoid.h
mrpt::graphslam::deciders::CLoopCloserERD::paths_t
std::vector< path_t > paths_t
Definition: CLoopCloserERD.h:245
w
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
mrpt::graphslam::deciders::CLoopCloserERD::CLoopCloserERD
CLoopCloserERD()
Definition: CLoopCloserERD_impl.h:27
p
GLfloat GLfloat p
Definition: glext.h:6305
MRPT_LOG_WARN_STREAM
#define MRPT_LOG_WARN_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:475
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:25
mrpt::graphslam::deciders::CLoopCloserERD::mahalanobisDistanceOdometryToICPEdge
bool mahalanobisDistanceOdometryToICPEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge)
brief Compare the suggested ICP edge against the initial node difference.
Definition: CLoopCloserERD_impl.h:1688
mrpt::graphslam::deciders::CLoopCloserERD::registerNewEdge
void registerNewEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, const constraint_t &rel_edge)
Definition: CLoopCloserERD_impl.h:1743
mrpt::graphslam::detail::TNodeProps::scan
mrpt::obs::CObservation2DRangeScan::Ptr scan
Definition: TNodeProps.h:25
mrpt::graphslam::deciders::CLoopCloserERD::fillNodePropsFromGroupParams
bool fillNodePropsFromGroupParams(const mrpt::graphs::TNodeID &nodeID, const std::map< mrpt::graphs::TNodeID, node_props_t > &group_params, node_props_t *node_props)
Fill the TNodeProps instance using the parameters from the map.
Definition: CLoopCloserERD_impl.h:326
MRPT_LOG_DEBUG_FMT
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
Definition: system/COutputLogger.h:461
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::graphslam::CWindowManager
Class acts as a container for storing pointers to mrpt::gui::CDisplayWindow3D, mrpt::graphslam::CWind...
Definition: CWindowManager.h:30
mrpt::graphslam::deciders::CLoopCloserERD::TLaserParams::goodness_threshold_win
TSlidingWindow goodness_threshold_win
Keep track of ICP Goodness values for ICP between nearby nodes and adapt the Goodness threshold based...
Definition: CLoopCloserERD.h:461
mrpt::graphslam::deciders::CLoopCloserERD::registerHypothesis
void registerHypothesis(const hypot_t &h)
Wrapper around the registerNewEdge method which accepts a THypothesis object instead.
Definition: CLoopCloserERD_impl.h:1735
mrpt::graphslam::deciders::CLoopCloserERD::TLaserParams::mahal_distance_ICP_odom_win
TSlidingWindow mahal_distance_ICP_odom_win
Keep track of the mahalanobis distance between the initial pose difference and the suggested new edge...
Definition: CLoopCloserERD.h:456
mrpt::graphslam::deciders::CLoopCloserERD::TLoopClosureParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form,...
Definition: CLoopCloserERD_impl.h:2624
mrpt::graphslam::deciders::CLoopCloserERD::m_edge_types_to_nums
std::map< std::string, int > m_edge_types_to_nums
Keep track of the registered edge types.
Definition: CLoopCloserERD.h:813
mrpt::graphslam::deciders::CLoopCloserERD::findPathByEnds
static const path_t * findPathByEnds(const paths_t &vec_paths, const mrpt::graphs::TNodeID &src, const mrpt::graphs::TNodeID &dst, bool throw_exc=true)
Given a vector of TUncertaintyPath objects, find the one that has the given source and destination no...
Definition: CLoopCloserERD_impl.h:1251
mrpt::graphslam::TUncertaintyPath::getDestination
const mrpt::graphs::TNodeID & getDestination() const
Return the Destination node of this path.
Definition: TUncertaintyPath_impl.h:238
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:192
dst
GLuint dst
Definition: glext.h:7135
source
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
mrpt::graphslam::deciders::CLoopCloserERD::TGetICPEdgeAdParams::from_params
node_props_t from_params
Ad.
Definition: CLoopCloserERD.h:297
mrpt::math::CMatrixTemplateNumeric< double >
mrpt::graphslam::deciders::CLoopCloserERD::getICPEdge
virtual bool getICPEdge(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, constraint_t *rel_edge, mrpt::slam::CICP::TReturnInfo *icp_info=NULL, const TGetICPEdgeAdParams *ad_params=NULL)
Get the ICP Edge between the provided nodes.
Definition: CLoopCloserERD_impl.h:254
mrpt::poses::CPose3DPDF::createFrom2D
static CPose3DPDF * createFrom2D(const CPosePDF &o)
This is a static transformation method from 2D poses to 3D PDFs, preserving the representation type (...
Definition: CPose3DPDF.cpp:34
mrpt::graphslam
SLAM methods related to graphs of pose constraints.
Definition: TUserOptionsChecker.h:33
mrpt::graphslam::deciders::CLoopCloserERD::getPropsOfNodeID
bool getPropsOfNodeID(const mrpt::graphs::TNodeID &nodeID, global_pose_t *pose, mrpt::obs::CObservation2DRangeScan::Ptr &scan, const node_props_t *node_props=NULL) const
Fill the pose and LaserScan for the given nodeID.
Definition: CLoopCloserERD_impl.h:359
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
mrpt::graphslam::TUncertaintyPath::addToPath
void addToPath(const mrpt::graphs::TNodeID &node, const constraint_t &edge)
add a new link in the current path.
Definition: TUncertaintyPath_impl.h:163
mrpt::graphslam::deciders::CLoopCloserERD::TGenerateHypotsPoolAdParams::group_t
std::map< mrpt::graphs::TNodeID, node_props_t > group_t
Definition: CLoopCloserERD.h:328
mrpt::graphslam::TUncertaintyPath::curr_pose_pdf
constraint_t curr_pose_pdf
Current path position + corresponding covariance.
Definition: TUncertaintyPath.h:109
mrpt::graphslam::deciders::CLoopCloserERD::printParams
void printParams() const
Definition: CLoopCloserERD_impl.h:2395
mrpt::graphslam::deciders::CLoopCloserERD::TGetICPEdgeAdParams::init_estim
pose_t init_estim
Initial ICP estimation.
Definition: CLoopCloserERD.h:299
mrpt::graphslam::deciders::CLoopCloserERD::updateVisuals
void updateVisuals()
Definition: CLoopCloserERD_impl.h:2249
mrpt::graphslam::deciders::CLoopCloserERD::checkPartitionsForLC
void checkPartitionsForLC(partitions_t *partitions_for_LC)
Check the registered so far partitions for potential loop closures.
Definition: CLoopCloserERD_impl.h:427
mrpt::graphslam::deciders::CLoopCloserERD::~CLoopCloserERD
virtual ~CLoopCloserERD()
Definition: CLoopCloserERD_impl.h:41
mrpt::obs::gnss::a2
double a2
Definition: gnss_messages_novatel.h:454
mrpt::math::approximatelyEqual
bool approximatelyEqual(T1 a, T1 b, T2 epsilon)
Compare 2 floats and determine whether they are equal.
Definition: math/include/mrpt/math/utils.h:34
mrpt::system::VerbosityLevel
VerbosityLevel
Enumeration of available verbosity levels.
Definition: system/COutputLogger.h:28
mrpt::graphslam::deciders::CLoopCloserERD::notifyOfWindowEvents
void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Definition: CLoopCloserERD_impl.h:1798
MRPT_START
#define MRPT_START
Definition: exceptions.h:262
mrpt::graphslam::deciders::CLoopCloserERD::hypot_t
typename mrpt::graphs::detail::THypothesis< GRAPH_T > hypot_t
Definition: CLoopCloserERD.h:239
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::format
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::graphslam::deciders::CLoopCloserERD::TLaserParams::~TLaserParams
~TLaserParams()
Definition: CLoopCloserERD_impl.h:2566
mrpt::graphslam::deciders::CLoopCloserERD::dumpVisibilityErrorMsg
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
Definition: CLoopCloserERD_impl.h:2349
res
GLuint res
Definition: glext.h:7268
mrpt::poses::CPose3D::asTPose
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1046
mrpt::graphslam::deciders::CLoopCloserERD::getMinUncertaintyPath
void getMinUncertaintyPath(const mrpt::graphs::TNodeID from, const mrpt::graphs::TNodeID to, path_t *path) const
Given two nodeIDs compute and return the path connecting them.
Definition: CLoopCloserERD_impl.h:1548
mrpt::img::TColorf
A RGB color - floats in the range [0,1].
Definition: TColor.h:79
mrpt::graphslam::deciders::CLoopCloserERD::updateState
virtual bool updateState(mrpt::obs::CActionCollection::Ptr action, mrpt::obs::CSensoryFrame::Ptr observations, mrpt::obs::CObservation::Ptr observation)
Definition: CLoopCloserERD_impl.h:58
mrpt::graphslam::TUncertaintyPath::getDeterminant
double getDeterminant()
Definition: TUncertaintyPath_impl.h:244
mrpt::obs::gnss::b2
double b2
Definition: gnss_messages_novatel.h:454
mrpt::graphslam::TSlidingWindow::resizeWindow
void resizeWindow(size_t new_size)
Resize the window.
Definition: TSlidingWindow.cpp:173
mrpt::obs::CSensoryFrame::insert
void insert(const CObservation::Ptr &obs)
Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the p...
Definition: CSensoryFrame.cpp:139
mrpt::graphslam::deciders::CLoopCloserERD::loadParams
void loadParams(const std::string &source_fname)
Definition: CLoopCloserERD_impl.h:2366
mrpt::graphslam::deciders::CLoopCloserERD::hypotsp_t
std::vector< hypot_t * > hypotsp_t
Definition: CLoopCloserERD.h:241
mrpt::graphslam::deciders::CLoopCloserERD::TGetICPEdgeAdParams::getAsString
void getAsString(std::string *str) const
Definition: CLoopCloserERD.h:301
mrpt::graphslam::deciders::CLoopCloserERD::initLaserScansVisualization
void initLaserScansVisualization()
Definition: CLoopCloserERD_impl.h:2112
mrpt::graphslam::deciders::CLoopCloserERD::constraint_t
typename GRAPH_T::constraint_t constraint_t
Handy typedefs.
Definition: CLoopCloserERD.h:228
mrpt::graphslam::deciders::CLoopCloserERD::getCurrentPartitions
const partitions_t & getCurrentPartitions() const
Definition: CLoopCloserERD_impl.h:2460
mrpt::opengl::CPlanarLaserScan::Ptr
std::shared_ptr< CPlanarLaserScan > Ptr
Definition: CPlanarLaserScan.h:59
mrpt::pbmap::inverse
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:82
mrpt::math::TSegment3D
3D segment, consisting of two points.
Definition: lightweight_geom_data.h:1044
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< CSetOfObjects > Ptr
Definition: CSetOfObjects.h:30
mrpt::gui
Classes for creating GUI windows for 2D and 3D visualization.
Definition: about_box.h:16
utils.h
mrpt::math::CMatrixFixedNumeric< double, 3, 3 >
mrpt::graphslam::deciders::CLoopCloserERD::TGenerateHypotsPoolAdParams::groupB_params
group_t groupB_params
Ad.
Definition: CLoopCloserERD.h:333
mrpt::graphslam::deciders::CLoopCloserERD::partitions_t
std::vector< std::vector< uint32_t > > partitions_t
Typedef for referring to a list of partitions.
Definition: CLoopCloserERD.h:236
mrpt::graphslam::TUncertaintyPath::nodes_traversed
std::vector< mrpt::graphs::TNodeID > nodes_traversed
Nodes that the Path comprises of.
Definition: TUncertaintyPath.h:107
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:378
mrpt::obs::CObservation2DRangeScan::Ptr
std::shared_ptr< CObservation2DRangeScan > Ptr
Definition: CObservation2DRangeScan.h:58
mrpt::graphslam::deciders::CLoopCloserERD::setWindowManagerPtr
void setWindowManagerPtr(mrpt::graphslam::CWindowManager *win_manager)
Definition: CLoopCloserERD_impl.h:1774
mrpt::graphslam::deciders::CLoopCloserERD::global_pose_t
typename GRAPH_T::global_pose_t global_pose_t
Definition: CLoopCloserERD.h:231
mrpt::graphslam::deciders::CLoopCloserERD::nodes_to_scans2D_t
typename parent_t::nodes_to_scans2D_t nodes_to_scans2D_t
Definition: CLoopCloserERD.h:234
mrpt::graphslam::deciders::CLoopCloserERD::generateHypotsPool
void generateHypotsPool(const std::vector< uint32_t > &groupA, const std::vector< uint32_t > &groupB, hypotsp_t *generated_hypots, const TGenerateHypotsPoolAdParams *ad_params=NULL)
Generate the hypothesis pool for all the inter-group constraints between two groups of nodes.
Definition: CLoopCloserERD_impl.h:749
mrpt::graphslam::deciders::CLoopCloserERD::updateMapPartitions
void updateMapPartitions(bool full_update=false, bool is_first_time_node_reg=false)
Split the currently registered graph nodes into partitions.
Definition: CLoopCloserERD_impl.h:2466
mrpt::graphslam::deciders::CLoopCloserERD::evaluatePartitionsForLC
void evaluatePartitionsForLC(const partitions_t &partitions)
Evaluate the given partitions for loop closures.
Definition: CLoopCloserERD_impl.h:541
MRPT_LOG_DEBUG_STREAM
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
Definition: system/COutputLogger.h:471
mrpt::graphslam::deciders::CLoopCloserERD::toggleMapPartitionsVisualization
void toggleMapPartitionsVisualization()
Toggle the map partitions visualization objects.
Definition: CLoopCloserERD_impl.h:2056
MRPT_LOG_ERROR_STREAM
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
Definition: system/COutputLogger.h:477
mrpt::graphslam::TUncertaintyPath::assertIsBetweenNodeIDs
void assertIsBetweenNodeIDs(const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to) const
Assert that the current path is between the given nodeIDs.
Definition: TUncertaintyPath_impl.h:80
mrpt::graphslam::deciders::CLoopCloserERD::computeDominantEigenVector
bool computeDominantEigenVector(const mrpt::math::CMatrixDouble &consist_matrix, mrpt::math::dynamic_vector< double > *eigvec, bool use_power_method=false)
Definition: CLoopCloserERD_impl.h:902
mrpt::graphslam::deciders::CLoopCloserERD::findHypotByEnds
static hypot_t * findHypotByEnds(const hypotsp_t &vec_hypots, const mrpt::graphs::TNodeID &from, const mrpt::graphs::TNodeID &to, bool throw_exc=true)
Given a vector of THypothesis objects, find the one that has the given start and end nodes.
Definition: CLoopCloserERD_impl.h:1283
mrpt::graphslam::detail
Internal auxiliary classes.
Definition: levmarq_impl.h:17
value
GLsizei const GLfloat * value
Definition: glext.h:4117
MRPT_END
#define MRPT_END
Definition: exceptions.h:266
mrpt::config::CConfigFile
This class allows loading and storing values and vectors of different types from "....
Definition: config/CConfigFile.h:33
ASSERTDEBMSG_
#define ASSERTDEBMSG_(f, __ERROR_MSG)
Definition: exceptions.h:208
mrpt::graphslam::deciders::CLoopCloserERD::fetchNodeIDsForScanMatching
virtual void fetchNodeIDsForScanMatching(const mrpt::graphs::TNodeID &curr_nodeID, std::set< mrpt::graphs::TNodeID > *nodes_set)
Fetch a list of nodes with regards prior to the given nodeID for which to try and add scan matching e...
Definition: CLoopCloserERD_impl.h:170
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:13
mrpt::graphslam::deciders::CLoopCloserERD::toggleLaserScansVisualization
void toggleLaserScansVisualization()
togle the LaserScans visualization on and off
Definition: CLoopCloserERD_impl.h:2184
mrpt::graphslam::deciders::CLoopCloserERD::TLaserParams::dumpToTextStream
void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form,...
Definition: CLoopCloserERD_impl.h:2571
mrpt::graphslam::deciders::CLoopCloserERD::initializeVisuals
void initializeVisuals()
Definition: CLoopCloserERD_impl.h:2221
mrpt::system::LVL_ERROR
@ LVL_ERROR
Definition: system/COutputLogger.h:33
ASSERTDEB_
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
mrpt::graphslam::deciders::CLoopCloserERD::addToPaths
void addToPaths(std::set< path_t * > *pool_of_paths, const path_t &curr_path, const std::set< mrpt::graphs::TNodeID > &neibors) const
Append the paths starting from the current node.
Definition: CLoopCloserERD_impl.h:1491
mrpt::obs::utils
Definition: obs_utils.h:20
string
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< COpenGLScene > Ptr
Definition: COpenGLScene.h:61
mrpt::graphslam::deciders::CLoopCloserERD::popMinUncertaintyPath
mrpt::graphslam::TUncertaintyPath< GRAPH_T > * popMinUncertaintyPath(std::set< path_t * > *pool_of_paths) const
Find the minimum uncertainty path from te given pool of TUncertaintyPath instances.
Definition: CLoopCloserERD_impl.h:1658
mrpt::graphslam::deciders::CLoopCloserERD::getDescriptiveReport
void getDescriptiveReport(std::string *report_str) const
Definition: CLoopCloserERD_impl.h:2419
mrpt::graphslam::deciders::CLoopCloserERD::generatePWConsistencyElement
double generatePWConsistencyElement(const mrpt::graphs::TNodeID &a1, const mrpt::graphs::TNodeID &a2, const mrpt::graphs::TNodeID &b1, const mrpt::graphs::TNodeID &b2, const hypotsp_t &hypots, const paths_t *opt_paths=NULL)
Return the pair-wise consistency between the observations of the given nodes.
Definition: CLoopCloserERD_impl.h:1128
mrpt::graphslam::deciders::CLoopCloserERD
Edge Registration Decider scheme specialized in Loop Closing.
Definition: CLoopCloserERD.h:218
mrpt::opengl::CEllipsoid::Ptr
std::shared_ptr< CEllipsoid > Ptr
Definition: CEllipsoid.h:49
iterator
Scalar * iterator
Definition: eigen_plugins.h:26
INVALID_NODEID
#define INVALID_NODEID
Definition: TNodeID.h:20
mrpt::graphslam::deciders::CLoopCloserERD::initCurrCovarianceVisualization
void initCurrCovarianceVisualization()
Definition: CLoopCloserERD_impl.h:2274
mrpt::graphslam::deciders::CLoopCloserERD::TGetICPEdgeAdParams::to_params
node_props_t to_params
Ad.
Definition: CLoopCloserERD.h:298
mrpt::obs::CActionCollection::Ptr
std::shared_ptr< CActionCollection > Ptr
Definition: CActionCollection.h:30
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
mrpt::obs::gnss::b1
double b1
Definition: gnss_messages_novatel.h:454
mrpt::graphslam::TUncertaintyPath
Holds the data of an information path.
Definition: TUncertaintyPath.h:31
mrpt::containers
Definition: bimap.h:15
stl_containers_utils.h
mrpt::graphslam::deciders::CLoopCloserERD::pose_t
typename GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
Definition: CLoopCloserERD.h:230
mrpt::graphslam::deciders::CLoopCloserERD::TGetICPEdgeAdParams
Struct for passing additional parameters to the getICPEdge call.
Definition: CLoopCloserERD.h:293
mrpt::graphslam::detail::TNodeProps
Definition: TNodeProps.h:22
params
GLenum const GLfloat * params
Definition: glext.h:3534
mrpt::graphslam::deciders::CLoopCloserERD::edges_citerator
typename GRAPH_T::edges_map_t::const_iterator edges_citerator
Definition: CLoopCloserERD.h:237
mrpt::graphslam::deciders::CLoopCloserERD::TGenerateHypotsPoolAdParams::groupA_params
group_t groupA_params
Ad.
Definition: CLoopCloserERD.h:331
mrpt::graphslam::deciders::CLoopCloserERD::computeCentroidOfNodesVector
void computeCentroidOfNodesVector(const std::vector< uint32_t > &nodes_list, std::pair< double, double > *centroid_coords) const
Compute the Centroid of a group of a vector of node positions.
Definition: CLoopCloserERD_impl.h:2084
mrpt::graphslam::deciders::CLoopCloserERD::findHypotByID
static hypot_t * findHypotByID(const hypotsp_t &vec_hypots, const size_t &id, bool throw_exc=true)
Given a vector of THypothesis objects, find the one that has the given ID.
Definition: CLoopCloserERD_impl.h:1314
mrpt::graphslam::deciders::CLoopCloserERD::generatePWConsistenciesMatrix
void generatePWConsistenciesMatrix(const std::vector< uint32_t > &groupA, const std::vector< uint32_t > &groupB, const hypotsp_t &hypots_pool, mrpt::math::CMatrixDouble *consist_matrix, const paths_t *groupA_opt_paths=NULL, const paths_t *groupB_opt_paths=NULL)
Compute the pair-wise consistencies Matrix.
Definition: CLoopCloserERD_impl.h:977
mrpt::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: core/include/mrpt/core/bits_math.h:42


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST