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()) {
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.
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
104  MRPT_LOG_WARN_STREAM("Assigning first laserScan to the graph root node.");
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] =
112 
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
120  (this->m_graph->nodeCount() %
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 
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) {
201  }
202  double goodness_thresh =
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 
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
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.
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());
514  groupA, groupB, hypots_pool, &consist_matrix);
515 
516  // evaluate resulting matrix - fill valid hypotheses
517  hypotsp_t valid_hypots;
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 =
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(
747  *b_it, ad_params->groupB_params, &icp_ad_params->from_params);
748  //to_success = 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 =
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) {
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) {
1617  "Toggle LaserScans Visualization");
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)) {
1637  }
1638  // map partitions
1639  if (events_occurred.at(m_lc_params.keystroke_map_partitions)) {
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
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  // ////////////////////////////////////////////////////////////
1682  std::stringstream title;
1683  title << "# Partitions: " << m_curr_partitions.size();
1685  title.str(),
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);
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,
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 
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
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(
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
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 
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;
2014  //MRPT_LOG_DEBUG_STREAM("Initializing visuals");
2015  this->m_time_logger.enter("Visuals");
2016 
2018  "Configuration parameters aren't loaded yet");
2021  }
2024  }
2025 
2028  }
2029 
2030  this->m_time_logger.leave("Visuals");
2031  MRPT_END;
2032 }
2033 template<class GRAPH_T>
2035  MRPT_START;
2037  //MRPT_LOG_DEBUG_STREAM("Updating visuals");
2038  this->m_time_logger.enter("Visuals");
2039 
2042  }
2045  }
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
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");
2067  title,
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 
2149  "EdgeRegistrationDeciderParameters");
2151  "EdgeRegistrationDeciderParameters");
2152  m_lc_params.loadFromConfigFileName(source_fname,
2153  "EdgeRegistrationDeciderParameters");
2154 
2155  mrpt::utils::CConfigFile source(source_fname);
2156 
2158  "EdgeRegistrationDeciderParameters",
2159  "consec_icp_constraint_factor",
2160  0.90, false);
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 
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).null()) { // 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++) {
2313  }
2314  //update current partitions list
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",
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);
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 = " <<
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);
2433  section,
2434  "LC_eigenvalues_ratio_thresh",
2435  2, false);
2437  section,
2438  "LC_check_curr_partition_only",
2439  true, false);
2441  section,
2442  "full_partition_per_nodes",
2443  50, false);
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.
partitions_t m_curr_partitions
Current partitions vector.
std::vector< mrpt::utils::TNodeID > nodes_traversed
Nodes that the Path comprises of.
mrpt::slam::CIncrementalMapPartitioner m_partitioner
Instance responsible for partitioning the map.
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
virtual void notifyOfWindowEvents(const std::map< std::string, bool > &events_occurred)
Get a list of the window events that happened since the last call.
void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
double goodness
Goodness value corresponding to the hypothesis edge.
Definition: THypothesis.h:97
virtual 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 unlockAccess3DScene()
Unlocks the access to the internal 3D scene.
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...
const mrpt::utils::TColor m_curr_node_covariance_color
void addToPath(const mrpt::utils::TNodeID &node, const constraint_t &edge)
add a new link in the current path.
const mrpt::utils::TColor laser_scans_color
see Constructor for initialization
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
mrpt::gui::CDisplayWindow3D * m_win
Window to use.
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.
double DEG2RAD(const double x)
Degrees to radians.
mrpt::obs::CObservation2DRangeScanPtr m_first_laser_scan
Keep track of the first recorded laser scan so that it can be assigned to the root node when the NRD ...
void initMapPartitionsVisualization()
Initialize the visualization of the map partition objects.
mrpt::graphs::detail::THypothesis< GRAPH_T > hypot_t
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.
void resizeWindow(size_t new_size)
Resize the window.
void assertIsBetweenNodeIDs(const mrpt::utils::TNodeID &from, const mrpt::utils::TNodeID &to) const
Assert that the current path is between the given nodeIDs.
bool m_partitions_full_update
Indicate whether the partitions have been updated recently.
This class allows loading and storing values and vectors of different types from ".ini" files easily.
Definition: CConfigFile.h:30
#define THROW_EXCEPTION(msg)
void updatePartitions(std::vector< vector_uint > &partitions)
This method executed only the neccesary part of the partition to take into account the lastest added ...
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
mrpt::obs::CObservation2DRangeScanPtr m_last_laser_scan2D
Keep the last laser scan for visualization purposes.
Scalar * iterator
Definition: eigen_plugins.h:23
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
static const std::string header_sep
Separator string to be used in debugging messages.
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.
mrpt::graphslam::CWindowObserver * m_win_observer
CWindowObserver object for monitoring various visual-oriented events.
mrpt::graphslam::TUncertaintyPath< GRAPH_T > * queryOptimalPath(const mrpt::utils::TNodeID node) const
Query for the optimal path of a nodeID.
STL namespace.
size_t m_last_total_num_nodes
Keep track of the total number of registered nodes since the last time class method was called...
bool use_scan_matching
Indicate whethet to use scan-matching at all during graphSLAM [on by default].
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.
TSlidingWindow mahal_distance_ICP_odom_win
Keep track of the mahalanobis distance between the initial pose difference and the suggested new edge...
GLuint src
Definition: glew.h:7126
void registerKeystroke(const std::string key_str, const std::string key_desc)
Make new keystrokes available in the help message box.
void addTextMessage(const double x, const double y, const std::string &text, const mrpt::utils::TColorf &color=mrpt::utils::TColorf(1.0, 1.0, 1.0), const size_t unique_index=0)
Wrapper around the CDisplayWindow3D::addTextMessage method, so that the user does not have to specify...
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.
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.
void getEdge(constraint_t *edge) const
Getter methods for the underlying edge.
Struct for passing additional parameters to the getICPEdge call.
void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
void registerHypothesis(const hypot_t &h)
Wrapper around the registerNewEdge method which accepts a THypothesis object instead.
std::map< mrpt::utils::TNodeID, path_t * > m_node_optimal_paths
Map that stores the lowest uncertainty path towards a node.
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...
GLsizei const GLfloat * value
Definition: glew.h:1756
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
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.
mrpt::graphslam::CWindowManager * m_win_manager
Pointer to the CWindowManager object used to store visuals-related instances.
double LC_eigenvalues_ratio_thresh
Eigenvalues ratio for accepting/rejecting a hypothesis set.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:137
void addNewMeasurement(double measurement)
Update the sliding window by appending a new measurement.
mrpt::opengl::COpenGLScenePtr & get3DSceneAndLock()
Gets a reference to the smart shared pointer that holds the internal scene (carefuly read introductio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void loadFromConfigFileName(const std::string &config_file, const std::string &section)
Behaves like loadFromConfigFile, but you can pass directly a file name and a temporary CConfigFile ob...
SLAM methods related to graphs of pose constraints.
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
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.
3D segment, consisting of two points.
uint64_t TNodeID
The type for node IDs in graphs of different types.
virtual void initializeVisuals()
Initialize visual objects in CDisplayWindow (e.g.
#define MRPT_END
bool m_override_registered_nodes_check
Indicates whether the ERD implementation expects, at most one single node to be registered, between successive calls to the updateState method.
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.
const partitions_t & getCurrPartitions() const
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
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.
std::map< int, vector_uint > m_partitionID_to_prev_nodes_list
Keep track of the evaluated partitions so they are not checked again if nothing changed in them...
GLuint dst
Definition: glew.h:4322
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...
virtual 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.
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
mrpt::slam::CIncrementalMapPartitioner::TOptions options
GLhandleARB obj
Definition: glew.h:3276
int prev_nodes_for_ICP
How many nodes back to check ICP against?
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
bool m_is_first_time_node_reg
Track the first node registration occurance.
#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...
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
virtual void setWindowManagerPtr(mrpt::graphslam::CWindowManager *win_manager)
Fetch a CWindowManager pointer.
GRAPH_T * m_graph
Pointer to the graph that is under construction.
GLfloat GLfloat p
Definition: glew.h:10113
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
int full_partition_per_nodes
Full partition of map only afer X new nodes have been registered.
void getInverseEdge(constraint_t *edge) const
Getter methods for the inverse of the underlying edge.
unsigned int addMapFrame(const mrpt::obs::CSensoryFramePtr &frame, const mrpt::poses::CPosePDFPtr &robotPose2D)
const mrpt::utils::TNodeID & getDestination() const
Return the Destination node of this path.
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 dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
virtual void printParams() const
Print the problem parameters - relevant to the decider/optimizer to the screen in a unified/compact w...
void getICPEdge(const mrpt::obs::CObservation2DRangeScan &from, const mrpt::obs::CObservation2DRangeScan &to, constraint_t *rel_edge, const mrpt::poses::CPose2D *initial_pose=NULL, mrpt::slam::CICP::TReturnInfo *icp_info=NULL)
Align the 2D range scans provided and fill the potential edge that can transform the one into the oth...
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.
GLuint res
Definition: glew.h:7143
void forceRepaint()
Repaints the window. forceRepaint, repaint and updateWindow are all aliases of the same method...
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.
bool LC_check_curr_partition_only
flag indicating whether to check only the partition of the last registered node for potential loop cl...
std::vector< mrpt::vector_uint > partitions_t
Typedef for referring to a list of partitions.
void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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.
mrpt::utils::CTimeLogger m_time_logger
Time logger instance.
const_iterator find(const KEY &key) const
Definition: ts_hash_map.h:139
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
std::string getAsString(bool oneline=true) const
Return a string representation of the object at hand.
virtual void updateVisuals()
Update the relevant visual features in CDisplayWindow.
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.
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.
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.
double m_lc_icp_constraint_factor
Factor used for accepting an ICP Constraint in the loop closure proc.
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.
#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.
GLfloat * params
Definition: glew.h:1436
double leave(const char *func_name)
End of a named section.
Definition: CTimeLogger.h:102
GRAPH_T::constraint_t::type_value pose_t
type of underlying poses (2D/3D).
std::string getStatsAsText(const size_t column_width=80) const
Dump all stats to a multi-line text string.
int LC_min_remote_nodes
how many remote nodes (large nodID difference should there be before I consider the potential loop cl...
std::map< mrpt::utils::TNodeID, node_props_t > group_t
virtual void getDescriptiveReport(std::string *report_str) const
Fill the provided string with a detailed report of the decider/optimizer state.
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.
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()
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
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.
size_t LC_min_nodeid_diff
nodeID difference for detecting potential loop closure in a partition.
An edge hypothesis between two nodeIDs.
Definition: THypothesis.h:33
Lightweight 3D point.
double m_consec_icp_constraint_factor
Factor used for accepting an ICP Constraint as valid.
void dumpVisibilityErrorMsg(std::string viz_flag, int sleep_time=500)
mrpt::graphslam::TUncertaintyPath< GRAPH_T > path_t
size_t m_dijkstra_node_count_thresh
Node Count lower bound before executing dijkstra.
void enter(const char *func_name)
Start of a named section.
Definition: CTimeLogger.h:97
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
#define ASSERTMSG_(f, __ERROR_MSG)
#define MRPT_LOG_ERROR_STREAM(__CONTENTS)
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.
nodes_to_scans2D_t m_nodes_to_laser_scans2D
Map for keeping track of the observation recorded at each graph position.
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
partitions_t m_last_partitions
Previous partitions vector.
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
double getMedian()
Return the current median value.
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.
void assignTextMessageParameters(double *offset_y, int *text_index)
Assign the next available offset_y and text_index for the textMessage under construction.
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
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.11 for MRPT 1.5.7 Git: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018