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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019