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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019