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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 1e710274b Thu May 28 15:30:14 2020 +0200 at jue may 28 15:45:10 CEST 2020