MRPT  2.0.1
CNetworkOfPoses.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 #pragma once
10 
11 /** \file The main class in this file is mrpt::poses::CNetworkOfPoses<>, a
12  generic
13  basic template for predefined 2D/3D graphs of pose contraints.
14 */
15 
16 #include <iostream>
17 
23 #include <mrpt/graphs/dijkstra.h>
26 #include <mrpt/math/utils.h>
28 #include <mrpt/poses/poses_frwds.h>
32 #include <mrpt/system/os.h>
33 
34 #include <algorithm>
35 #include <iterator>
36 #include <memory>
37 #include <optional>
38 #include <type_traits> // is_base_of()
39 
40 namespace mrpt
41 {
42 namespace graphs
43 {
44 /** Internal functions for MRPT */
45 namespace detail
46 {
47 template <class GRAPH_T>
48 struct graph_ops;
49 
50 // forward declaration of CVisualizer
51 template <
52  class CPOSE, // Type of edges
53  class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
55 // forward declaration of CMRVisualizer
56 template <
57  class CPOSE, // Type of edges
58  class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS, class EDGE_ANNOTATIONS>
59 class CMRVisualizer;
60 } // namespace detail
61 
62 /** A directed graph of pose constraints, with edges being the relative poses
63  *between pairs of nodes identified by their numeric IDs (of type
64  *mrpt::graphs::TNodeID).
65  * A link or edge between two nodes "i" and "j", that is, the pose \f$ p_{ij}
66  *\f$, holds the relative position of "j" with respect to "i".
67  * These poses are stored in the edges in the format specified by the template
68  *argument CPOSE. Users should employ the following derived classes
69  * depending on the desired representation of edges:
70  * - mrpt::graphs::CNetworkOfPoses2D : 2D edges as a simple CPose2D (x y
71  *phi)
72  * - mrpt::graphs::CNetworkOfPoses3D : 3D edges as a simple
73  *mrpt::poses::CPose3D (x y z yaw pitch roll)
74  * - mrpt::graphs::CNetworkOfPoses2DInf : 2D edges as a Gaussian PDF with
75  *information matrix (CPosePDFGaussianInf)
76  * - mrpt::graphs::CNetworkOfPoses3DInf : 3D edges as a Gaussian PDF with
77  *information matrix (CPose3DPDFGaussianInf)
78  * - mrpt::graphs::CNetworkOfPoses2DCov : 2D edges as a Gaussian PDF with
79  *covariance matrix (CPosePDFGaussian). It's more efficient to use the
80  *information matrix version instead!
81  * - mrpt::graphs::CNetworkOfPoses3DCov : 3D edges as a Gaussian PDF with
82  *covariance matrix (CPose3DPDFGaussian). It's more efficient to use the
83  *information matrix version instead!
84  *
85  * Two main members store all the information in this class:
86  * - \a edges (in the base class mrpt::graphs::CDirectedGraph::edges): A
87  *map
88  *from pairs of node ID -> pose constraints.
89  * - \a nodes : A map from node ID -> estimated pose of that node
90  *(actually,
91  *read below on the template argument MAPS_IMPLEMENTATION).
92  *
93  * Graphs can be loaded and saved to text file in the format used by TORO &
94  *HoG-man (more on the format <a
95  *href="http://www.mrpt.org/Robotics_file_formats" >here</a>),
96  * using \a loadFromTextFile and \a saveToTextFile.
97  *
98  * This class is the base for representing networks of poses, which are the
99  *main data type of a series
100  * of SLAM algorithms implemented in the library mrpt-slam, in the namespace
101  *mrpt::graphslam.
102  *
103  * The template arguments are:
104  * - CPOSE: The type of the edges, which hold a relative pose (2D/3D, just
105  *a
106  *value or a Gaussian, etc.)
107  * - MAPS_IMPLEMENTATION: Can be either mrpt::containers::map_traits_stdmap
108  *or mrpt::containers::map_traits_map_as_vector. Determines the type of the list
109  *of global poses (member \a nodes).
110  *
111  * \sa mrpt::graphslam
112  * \ingroup mrpt_graphs_grp
113  */
114 template <
115  class CPOSE, // Type of edges
116  class MAPS_IMPLEMENTATION =
117  mrpt::containers::map_traits_stdmap, // Use std::map<> vs.
118  // std::vector<>
119  class NODE_ANNOTATIONS = mrpt::graphs::detail::TNodeAnnotationsEmpty,
120  class EDGE_ANNOTATIONS = mrpt::graphs::detail::edge_annotations_empty>
122  : public mrpt::graphs::CDirectedGraph<CPOSE, EDGE_ANNOTATIONS>
123 {
124  public:
125  /** @name Typedef's
126  @{ */
127  /** The base class "CDirectedGraph<CPOSE,EDGE_ANNOTATIONS>" */
129  /** My own type */
130  using self_t = CNetworkOfPoses<
131  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
132 
133  /** The type of PDF poses in the contraints (edges) (=CPOSE template
134  * argument) */
135  using constraint_t = CPOSE;
136  /** The extra annotations in nodes, apart from a \a constraint_no_pdf_t */
137  using node_annotations_t = NODE_ANNOTATIONS;
138  /** The extra annotations in edges, apart from a \a constraint_t */
139  using edge_annotations_t = EDGE_ANNOTATIONS;
140 
141  /** The type of map's implementation (=MAPS_IMPLEMENTATION template
142  * argument) */
143  using maps_implementation_t = MAPS_IMPLEMENTATION;
144  /** The type of edges or their means if they are PDFs (that is, a simple
145  * "edge" value) */
146  using constraint_no_pdf_t = typename CPOSE::type_value;
147 
148  constexpr static auto getClassName()
149  {
150  using namespace mrpt::typemeta;
151  return literal("mrpt::graphs::CNetworkOfPoses<") +
152  TTypeName<CPOSE>::get() + literal(",") +
156  }
157 
158  /** The type of each global pose in \a nodes: an extension of the \a
159  * constraint_no_pdf_t pose with any optional user-defined data
160  */
161  struct global_pose_t : public constraint_no_pdf_t, public NODE_ANNOTATIONS
162  {
163  using self_t = typename CNetworkOfPoses<
164  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS,
165  EDGE_ANNOTATIONS>::global_pose_t;
166 
167  constexpr static auto getClassName()
168  {
169  using namespace mrpt::typemeta;
170  return literal("global_pose_t<") +
173  }
174 
175  /**\brief Potential class constructors
176  */
177  /**\{ */
178  inline global_pose_t() = default;
179 
180  template <typename ARG1>
181  inline global_pose_t(const ARG1& a1) : constraint_no_pdf_t(a1)
182  {
183  }
184  template <typename ARG1, typename ARG2>
185  inline global_pose_t(const ARG1& a1, const ARG2& a2)
187  {
188  }
189  /**\} */
190 
191  inline bool operator==(const global_pose_t& other) const
192  {
193  return (
194  static_cast<const constraint_no_pdf_t>(*this) ==
195  static_cast<const constraint_no_pdf_t>(other) &&
196  static_cast<const NODE_ANNOTATIONS>(*this) ==
197  static_cast<const NODE_ANNOTATIONS>(other));
198  }
199  inline bool operator!=(const global_pose_t& other) const
200  {
201  return (!(*this == other));
202  }
203 
204  inline friend std::ostream& operator<<(
205  std::ostream& o, const self_t& global_pose)
206  {
207  o << global_pose.asString() << "| "
208  << global_pose.retAnnotsAsString();
209  return o;
210  }
211  };
212 
213  /** A map from pose IDs to their global coordinate estimates, with
214  * uncertainty */
215  using global_poses_pdf_t = typename MAPS_IMPLEMENTATION::template map<
217 
218  /** A map from pose IDs to their global coordinate estimates, without
219  * uncertainty (the "most-likely value") */
220  using global_poses_t = typename MAPS_IMPLEMENTATION::template map<
222 
223  /** @} */
224 
225  /** @name Data members
226  @{ */
227 
228  /** The nodes (vertices) of the graph, with their estimated "global"
229  * (with respect to \a root) position, without an associated covariance.
230  * \sa dijkstra_nodes_estimate
231  */
233 
234  /** The ID of the node that is the origin of coordinates, used as
235  * reference by all coordinates in \a nodes. By default, root is the ID
236  * "0". */
238 
239  /** False (default) if an edge i->j stores the normal relative pose of j
240  * as seen from i: \f$ \Delta_i^j = j \ominus i \f$ True if an edge i->j
241  * stores the inverse relateive pose, that is, i as seen from j: \f$
242  * \Delta_i^j = i \ominus j \f$
243  */
245 
246  /** @} */
247 
248  /** @name I/O methods
249  @{ */
250 
251  /** Saves to a text file in the format used by TORO, HoG-man, G2O.
252  * See: https://www.mrpt.org/Graph-SLAM_maps
253  * \sa saveToBinaryFile, loadFromTextFile, writeAsText
254  * \exception On any error
255  */
256  void saveToTextFile(const std::string& fileName) const
257  {
259  this, fileName);
260  }
261  /** Writes as text in the format used by TORO, HoG-man, G2O.
262  * See: https://www.mrpt.org/Graph-SLAM_maps
263  * \sa saveToBinaryFile, loadFromTextFile, saveToTextFile, readAsText
264  * \exception On any error
265  */
266  void writeAsText(std::ostream& o) const
267  {
269  }
270 
271  /** Loads from a text file in the format used by TORO & HoG-man (more on the
272  * format <a href="http://www.mrpt.org/Robotics_file_formats" >here</a>)
273  * Recognized line entries are: VERTEX2, VERTEX3, EDGE2, EDGE3, EQUIV.
274  * If an unknown entry is found, a warning is dumped to std::cerr (only
275  * once for each unknown keyword).
276  * An exception will be raised if trying to load a 3D graph into a 2D
277  * class (in the opposite case, missing 3D data will default to zero).
278  *
279  * \param[in] fileName The file to load.
280  * \param[in] collapse_dup_edges If true, \a collapseDuplicatedEdges will be
281  * called automatically after loading (note that this operation may take
282  * significant time for very large graphs).
283  *
284  * \sa loadFromBinaryFile, saveToTextFile
285  *
286  * \exception On any error, as a malformed line or loading a 3D graph in
287  * a 2D graph.
288  */
289  inline void loadFromTextFile(
290  const std::string& fileName, bool collapse_dup_edges = true)
291  {
293  this, fileName);
294  if (collapse_dup_edges) this->collapseDuplicatedEdges();
295  }
296 
297  /** Reads as text in the format used by TORO, HoG-man, G2O.
298  * See: https://www.mrpt.org/Graph-SLAM_maps
299  * \sa saveToBinaryFile, loadFromTextFile, saveToTextFile
300  * \exception On any error
301  */
302  void readAsText(std::istream& i)
303  {
305  this, i);
306  }
307 
308  /** @} */
309 
310  /** @name Utility methods
311  @{ */
312  /**\brief Return 3D Visual Representation of the edges and nodes in the
313  * network of poses
314  *
315  * Method makes the call to the corresponding method of the CVisualizer
316  * class instance.
317  */
318  inline void getAs3DObject(
320  const mrpt::system::TParametersDouble& viz_params) const
321  {
322  using visualizer_t = mrpt::graphs::detail::CVisualizer<
323  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
324  using visualizer_multirobot_t = mrpt::graphs::detail::CMRVisualizer<
325  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
326 
327  bool is_multirobot = false;
328  std::unique_ptr<visualizer_t> viz;
329  is_multirobot =
330  (std::is_base_of_v<
332  if (is_multirobot)
333  {
334  viz.reset(new visualizer_multirobot_t(*this));
335  }
336  else
337  {
338  viz.reset(new visualizer_t(*this));
339  }
340  viz->getAs3DObject(object, viz_params);
341  }
342 
343  /** Spanning tree computation of a simple estimation of the global
344  * coordinates of each node just from the information in all edges,
345  * sorted in a Dijkstra tree based on the current "root" node.
346  *
347  * \note The "global" coordinates are with respect to the node with the
348  * ID specified in \a root.
349  *
350  * \note This method takes into account the
351  * value of \a edges_store_inverse_poses
352  *
353  * \sa node, root
354  */
356  std::optional<std::reference_wrapper<std::map<TNodeID, size_t>>>
357  topological_distances = std::nullopt)
358  {
360  this, topological_distances ? &topological_distances.value().get()
361  : nullptr);
362  }
363 
364  /** Look for duplicated edges (even in opposite directions) between all
365  * pairs of nodes and fuse them. Upon return, only one edge remains
366  * between each pair of nodes with the mean & covariance (or information
367  * matrix) corresponding to the Bayesian fusion of all the Gaussians.
368  *
369  * \return Overall number of removed edges.
370  */
371  inline size_t collapseDuplicatedEdges()
372  {
374  this);
375  }
376 
377  /** Returns the total chi-squared error of the graph. Shortcut for
378  * getGlobalSquareError(false). */
379  double chi2() const { return getGlobalSquareError(false); }
380  /** Evaluates the graph total square error (ignoreCovariances=true) or
381  * chi2 (ignoreCovariances=false) from all the pose constraints (edges)
382  * with respect to the global poses in \a nodes.
383  * \sa getEdgeSquareError
384  * \exception std::exception On global poses not in \a nodes
385  */
386  double getGlobalSquareError(bool ignoreCovariances = true) const
387  {
388  double sqErr = 0;
389  for (auto it = BASE::edges.begin(); it != BASE::edges.end(); ++it)
391  this, it, ignoreCovariances);
392  return sqErr;
393  }
394 
395  /**\brief Find the edges between the nodes in the node_IDs set and fill
396  * given graph pointer accordingly.
397  *
398  * \param[in] node_IDs Set of nodes, between which, edges should be found
399  * and inserted in the given sub_graph pointer
400  * \param[in] root_node_in Node ID to be used as the root node of
401  * sub_graph. If this is not given, the lowest nodeID is to be used.
402  * \param[out] CNetworkOfPoses pointer that is to be filled.
403  *
404  * \param[in] auto_expand_set If true and in case the node_IDs set
405  * contains non-consecutive nodes the returned set is expanded with the
406  * in-between nodes. This makes sure that the final graph is always
407  * connected.
408  * If auto_expand_set is false but there exist
409  * non-consecutive nodes, virtual edges are inserted in the parts that
410  * the graph is not connected
411  */
413  const std::set<TNodeID>& node_IDs, self_t* sub_graph,
414  const TNodeID root_node_in = INVALID_NODEID,
415  bool auto_expand_set = true) const
416  {
417  using namespace std;
418  using namespace mrpt;
419  using namespace mrpt::math;
420  using namespace mrpt::graphs::detail;
421 
422  using dijkstra_t = CDijkstra<self_t, MAPS_IMPLEMENTATION>;
423 
424  // assert that the given pointers are valid
425  ASSERTMSG_(
426  sub_graph,
427  "\nInvalid pointer to a CNetworkOfPoses instance is given. "
428  "Exiting..\n");
429  sub_graph->clear();
430 
431  // assert that the root_node actually exists in the given node_IDs set
432  TNodeID root_node = root_node_in;
433  if (root_node != INVALID_NODEID)
434  {
435  ASSERTMSG_(
436  node_IDs.find(root_node) != node_IDs.end(),
437  "\nRoot_node does not exist in the given node_IDs set. "
438  "Exiting.\n");
439  }
440 
441  // ask for at least 2 nodes
442  ASSERTMSG_(
443  node_IDs.size() >= 2,
444  format(
445  "Very few nodes [%lu] for which to extract a subgraph. "
446  "Exiting\n",
447  static_cast<unsigned long>(node_IDs.size())));
448 
449  // find out if querry contains non-consecutive nodes.
450  // Assumption: Set elements are in standard, ascending order.
451  bool is_fully_connected_graph = true;
452  std::set<TNodeID> node_IDs_real; // actual set of nodes to be used.
453  if (*node_IDs.rbegin() - *node_IDs.begin() + 1 == node_IDs.size())
454  {
455  node_IDs_real = node_IDs;
456  }
457  else
458  { // contains non-consecutive nodes
459  is_fully_connected_graph = false;
460 
461  if (auto_expand_set)
462  { // set auto-expansion
463  for (TNodeID curr_node_ID = *node_IDs.begin();
464  curr_node_ID != *node_IDs.rbegin(); ++curr_node_ID)
465  {
466  node_IDs_real.insert(curr_node_ID);
467  }
468  }
469  else
470  { // virtual_edge_addition strategy
471  node_IDs_real = node_IDs;
472  }
473  }
474 
475  // add all the nodes of the node_IDs_real set to sub_graph
476  for (unsigned long node_IDs_it : node_IDs_real)
477  {
478  // assert that current node exists in *own* graph
479  typename global_poses_t::const_iterator own_it;
480  for (own_it = nodes.begin(); own_it != nodes.end(); ++own_it)
481  {
482  if (node_IDs_it == own_it->first)
483  {
484  break; // I throw exception afterwards
485  }
486  }
487  ASSERTMSG_(
488  own_it != nodes.end(),
489  format(
490  "NodeID [%lu] can't be found in the initial graph.",
491  static_cast<unsigned long>(node_IDs_it)));
492 
493  global_pose_t curr_node(nodes.at(node_IDs_it));
494  sub_graph->nodes.insert(make_pair(node_IDs_it, curr_node));
495  }
496  // cout << "Extracting subgraph for nodeIDs: " <<
497  // getSTLContainerAsString(node_IDs_real) << endl;
498 
499  // set the root of the extracted graph
500  if (root_node == INVALID_NODEID)
501  {
502  // smallest nodeID by default: already ordered inside std::map
503  root_node = sub_graph->nodes.begin()->first;
504  }
505  sub_graph->root = root_node;
506 
507  // find all edges (in the initial graph), that exist in the given set
508  // of nodes; add them to the given graph
509  sub_graph->clearEdges();
510  for (const auto& e : BASE::edges)
511  {
512  const TNodeID& from = e.first.first;
513  const TNodeID& to = e.first.second;
514  const typename BASE::edge_t& curr_edge = e.second;
515 
516  // if both nodes exist in the given set, add the corresponding edge
517  if (sub_graph->nodes.find(from) != sub_graph->nodes.end() &&
518  sub_graph->nodes.find(to) != sub_graph->nodes.end())
519  {
520  sub_graph->insertEdge(from, to, curr_edge);
521  }
522  }
523 
524  if (!auto_expand_set && !is_fully_connected_graph)
525  {
526  // Addition of virtual edges between non-connected graph parts is
527  // necessary
528 
529  // make sure that the root nodeID is connected to at least one node
530  {
531  std::set<TNodeID> root_neighbors;
532  sub_graph->getNeighborsOf(sub_graph->root, root_neighbors);
533 
534  if (root_neighbors.empty())
535  {
536  // add an edge between the root and an adjacent nodeID
537  typename global_poses_t::iterator root_it =
538  sub_graph->nodes.find(sub_graph->root);
539  ASSERT_(root_it != sub_graph->nodes.end());
540  if ((*root_it == *sub_graph->nodes.rbegin()))
541  { // is the last nodeID
542  // add with previous node
543  TNodeID next_to_root = (--root_it)->first;
545  sub_graph, next_to_root, sub_graph->root);
546  // cout << "next_to_root = " << next_to_root;
547  }
548  else
549  {
550  TNodeID next_to_root = (++root_it)->first;
551  // cout << "next_to_root = " << next_to_root;
553  sub_graph, sub_graph->root, next_to_root);
554  }
555  }
556  }
557 
558  // as long as the graph is unconnected (as indicated by Dijkstra)
559  // add a virtual edge between
560  bool dijkstra_runs_successfully = false;
561 
562  // loop until the graph is fully connected (i.e. I can reach every
563  // node of the graph starting from its root)
564  while (!dijkstra_runs_successfully)
565  {
566  try
567  {
568  dijkstra_t dijkstra(*sub_graph, sub_graph->root);
569  dijkstra_runs_successfully = true;
570  }
572  {
573  dijkstra_runs_successfully = false;
574 
575  set<TNodeID> unconnected_nodeIDs;
576  ex.getUnconnectedNodeIDs(&unconnected_nodeIDs);
577  // cout << "Unconnected nodeIDs: " <<
578  // mrpt::math::getSTLContainerAsString(unconnected_nodeIDs)
579  // << endl;
580  // mainland: set of nodes that the root nodeID is in
581  // island: set of nodes that the Dijkstra graph traversal
582  // can't
583  // reach starting from the root.
584  // [!] There may be multiple sets of these nodes
585 
586  // set::rend() is the element with the highest value
587  // set::begin() is the element with the lowest value
588  const TNodeID& island_highest =
589  *unconnected_nodeIDs.rbegin();
590  const TNodeID& island_lowest = *unconnected_nodeIDs.begin();
591  // cout << "island_highest: " << island_highest << endl;
592  // cout << "island_lowest: " << island_lowest << endl;
593  // cout << "root: " << sub_graph->root << endl;
594 
595  // find out which nodes are in the same partition with the
596  // root
597  // (i.e. mainland)
598  std::set<TNodeID> mainland;
599  // for all nodes in sub_graph
600  for (typename global_poses_t::const_iterator n_it =
601  sub_graph->nodes.begin();
602  n_it != sub_graph->nodes.end(); ++n_it)
603  {
604  bool is_there = false;
605 
606  // for all unconnected nodes
607  for (unsigned long unconnected_nodeID :
608  unconnected_nodeIDs)
609  {
610  if (n_it->first == unconnected_nodeID)
611  {
612  is_there = true;
613  break;
614  }
615  }
616 
617  if (!is_there)
618  {
619  mainland.insert(n_it->first);
620  }
621  }
622 
623  bool is_single_island =
624  (island_highest - island_lowest + 1 ==
625  unconnected_nodeIDs.size());
626 
627  if (is_single_island)
628  { // single island
629  // Possible scenarios:
630  // | island |
631  // | mainland |
632  // | <low nodeIDs> island_highest| --- <virtual_edge>
633  // --->> | mainland_lowest <high nodeIDs> ... root ...|
634  // --- OR ---
635  // | mainland |
636  // | island |
637  // | <low nodeIDs> mainland_highest| ---
638  // <virtual_edge> --->> | island_lowest <high nodeIDs>
639  // |
640 
641  const std::set<TNodeID>& island = unconnected_nodeIDs;
643  sub_graph, island, mainland);
644  }
645  else
646  { // multiple islands
647  // add a virtual edge between the last group before the
648  // mainland and the mainland
649 
650  // split the unconnected_nodeIDs to smaller groups of
651  // nodes
652  // we only care about the nodes that are prior to the
653  // root
654  std::vector<std::set<TNodeID>> vec_of_islands;
655  std::set<TNodeID> curr_island;
656  TNodeID prev_nodeID = *unconnected_nodeIDs.begin();
657  curr_island.insert(
658  prev_nodeID); // add the initial node;
659  for (auto it = ++unconnected_nodeIDs.begin();
660  *it < sub_graph->root &&
661  it != unconnected_nodeIDs.end();
662  ++it)
663  {
664  if (!(absDiff(*it, prev_nodeID) == 1))
665  {
666  vec_of_islands.push_back(curr_island);
667  curr_island.clear();
668  }
669  curr_island.insert(*it);
670 
671  // update the previous nodeID
672  prev_nodeID = *it;
673  }
674  vec_of_islands.push_back(curr_island);
675 
676  // cout << "last_island: " <<
677  // getSTLContainerAsString(vec_of_islands.back()) <<
678  // endl;
679  // cout << "mainland: " <<
680  // getSTLContainerAsString(mainland) << endl;
682  sub_graph, vec_of_islands.back(), mainland);
683  }
684  }
685  }
686  }
687 
688  // estimate the node positions according to the edges - root is (0, 0,
689  // 0)
690  // just execute dijkstra once for grabbing the updated node positions.
691  sub_graph->dijkstra_nodes_estimate();
692 
693  } // end of extractSubGraph
694 
695  /**\brief Integrate given graph into own graph using the list of provided
696  * common THypotheses. Nodes of the other graph are renumbered upon
697  * integration in own graph.
698  *
699  * \param[in] other Graph (of the same type) that is to be integrated with
700  * own graph.
701  * \param[in] Hypotheses that join own and other graph.
702  * \param[in] hypots_from_other_to_self Specify the direction of the
703  * THypothesis objects in the common_hypots. If true (default) they are
704  * directed from other to own graph (other \rightarrow own),
705  *
706  * \param[out] old_to_new_nodeID_mappings_out Map from the old nodeIDs
707  * that are in the given graph to the new nodeIDs that have been inserted
708  * (by this method) in own graph.
709  */
710  inline void mergeGraph(
711  const self_t& other,
712  const typename std::vector<detail::THypothesis<self_t>>& common_hypots,
713  const bool hypots_from_other_to_self = true,
714  std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings_out = nullptr)
715  {
716  MRPT_START
717  using namespace mrpt::graphs;
718  using namespace mrpt::graphs::detail;
719  using namespace std;
720 
721  using hypots_cit_t =
722  typename vector<THypothesis<self_t>>::const_iterator;
723  using nodes_cit_t = typename global_poses_t::const_iterator;
724 
725  const self_t& graph_from = (hypots_from_other_to_self ? other : *this);
726  const self_t& graph_to = (hypots_from_other_to_self ? *this : other);
727 
728  // assert that both own and other graph have at least two nodes.
729  ASSERT_(graph_from.nodes.size() >= 2);
730  ASSERT_(graph_to.nodes.size() >= 2);
731 
732  // Assert that from-nodeIds, to-nodeIDs in common_hypots exist in own
733  // and other graph respectively
734  for (hypots_cit_t h_cit = common_hypots.begin();
735  h_cit != common_hypots.end(); ++h_cit)
736  {
737  ASSERTMSG_(
738  graph_from.nodes.find(h_cit->from) != graph_from.nodes.end(),
739  format("NodeID %lu is not found in (from) graph", h_cit->from));
740  ASSERTMSG_(
741  graph_to.nodes.find(h_cit->to) != graph_to.nodes.end(),
742  format("NodeID %lu is not found in (to) graph", h_cit->to));
743  }
744 
745  // find the max nodeID in existing graph
746  TNodeID max_nodeID = 0;
747  for (nodes_cit_t n_cit = this->nodes.begin();
748  n_cit != this->nodes.end(); ++n_cit)
749  {
750  if (n_cit->first > max_nodeID)
751  {
752  max_nodeID = n_cit->first;
753  }
754  }
755  TNodeID renum_start = max_nodeID + 1;
756  size_t renum_counter = 0;
757  // cout << "renum_start: " << renum_start << endl;
758 
759  // Renumber nodeIDs of other graph so that they don't overlap with own
760  // graph nodeIDs
761  std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings;
762 
763  // map of TNodeID->TNodeID correspondences to address to if the
764  // old_to_new_nodeID_mappings_out is not given.
765  // Handy for not having to allocate old_to_new_nodeID_mappings in the
766  // heap
767  std::map<TNodeID, TNodeID> mappings_tmp;
768 
769  // If given, use the old_to_new_nodeID_mappings map.
770  if (old_to_new_nodeID_mappings_out)
771  {
772  old_to_new_nodeID_mappings = old_to_new_nodeID_mappings_out;
773  }
774  else
775  {
776  old_to_new_nodeID_mappings = &mappings_tmp;
777  }
778  old_to_new_nodeID_mappings->clear();
779 
780  // add all nodes of other graph - Take care of renumbering them
781  // cout << "Adding nodes of other graph" << endl;
782  // cout << "====================" << endl;
783  for (nodes_cit_t n_cit = other.nodes.begin();
784  n_cit != other.nodes.end(); ++n_cit)
785  {
786  TNodeID new_nodeID = renum_start + renum_counter++;
787  old_to_new_nodeID_mappings->insert(
788  make_pair(n_cit->first, new_nodeID));
789  this->nodes.insert(make_pair(new_nodeID, n_cit->second));
790 
791  // cout << "Adding nodeID: " << new_nodeID << endl;
792  }
793 
794  // add common constraints
795  // cout << "Adding common constraints" << endl;
796  // cout << "====================" << endl;
797  for (hypots_cit_t h_cit = common_hypots.begin();
798  h_cit != common_hypots.end(); ++h_cit)
799  {
800  TNodeID from, to;
801  if (hypots_from_other_to_self)
802  {
803  from = old_to_new_nodeID_mappings->at(h_cit->from);
804  to = h_cit->to;
805  }
806  else
807  {
808  from = h_cit->from;
809  to = old_to_new_nodeID_mappings->at(h_cit->to);
810  }
811  this->insertEdge(from, to, h_cit->getEdge());
812  // cout << from << " -> " << to << " => " << h_cit->getEdge() <<
813  // endl;
814  }
815 
816  // add all constraints of the other graph
817  // cout << "Adding constraints of other graph" << endl;
818  // cout << "====================" << endl;
819  for (typename self_t::const_iterator g_cit = other.begin();
820  g_cit != other.end(); ++g_cit)
821  {
822  TNodeID new_from =
823  old_to_new_nodeID_mappings->at(g_cit->first.first);
824  TNodeID new_to =
825  old_to_new_nodeID_mappings->at(g_cit->first.second);
826  this->insertEdge(new_from, new_to, g_cit->second);
827 
828  // cout << "[" << new_from << "] -> [" << new_to << "]" << " => " <<
829  // g_cit->second << endl;
830  }
831 
832  // run Dijkstra to update the node positions
833  this->dijkstra_nodes_estimate();
834 
835  MRPT_END
836  }
837 
838  /**\brief Add an edge between the last node of the group with the lower
839  * nodeIDs and the first node of the higher nodeIDs.
840  *
841  * Given groups of nodes should only contain consecutive nodeIDs and
842  * there should be no overlapping between them
843  *
844  * \note It is assumed that the sets of nodes are \b already in ascending
845  * order (default std::set behavior.
846  */
847  inline static void connectGraphPartitions(
848  self_t* sub_graph, const std::set<TNodeID>& groupA,
849  const std::set<TNodeID>& groupB)
850  {
851  using namespace mrpt::math;
852 
853  ASSERTMSG_(
854  sub_graph,
855  "\nInvalid pointer to a CNetworkOfPoses instance is given. "
856  "Exiting..\n");
857  ASSERTMSG_(!groupA.empty(), "\ngroupA is empty.");
858  ASSERTMSG_(!groupB.empty(), "\ngroupB is empty.");
859 
860  // assertion - non-overlapping groups
861  ASSERTMSG_(
862  *groupA.rend() < *groupB.rbegin() ||
863  *groupA.rbegin() > *groupB.rend(),
864  "Groups A, B contain overlapping nodeIDs");
865 
866  // decide what group contains the low/high nodeIDs
867  // just compare any two nodes of the sets (they are non-overlapping
868  const std::set<TNodeID>& low_nodeIDs =
869  *groupA.rbegin() < *groupB.rbegin() ? groupA : groupB;
870  const std::set<TNodeID>& high_nodeIDs =
871  *groupA.rbegin() > *groupB.rbegin() ? groupA : groupB;
872 
873  // add virtual edge
874  const TNodeID& from_nodeID = *low_nodeIDs.rbegin();
875  const TNodeID& to_nodeID = *high_nodeIDs.begin();
876  self_t::addVirtualEdge(sub_graph, from_nodeID, to_nodeID);
877  }
878 
879  /** Computes the square error of one pose constraints (edge) with respect
880  * to the global poses in \a nodes If \a ignoreCovariances is false, the
881  * squared Mahalanobis distance will be computed instead of the straight
882  * square error.
883  *
884  * \exception std::exception On global poses not in \a nodes
885  */
886  inline double getEdgeSquareError(
887  const typename BASE::edges_map_t::const_iterator& itEdge,
888  bool ignoreCovariances = true) const
889  {
891  this, itEdge, ignoreCovariances);
892  }
893 
894  /** Computes the square error of one pose constraints (edge) with respect
895  * to the global poses in \a nodes If \a ignoreCovariances is false, the
896  * squared Mahalanobis distance will be computed instead of the straight
897  * square error.
898  *
899  * \exception std::exception On edge not existing or global poses not in
900  * \a nodes
901  */
903  const mrpt::graphs::TNodeID from_id, const mrpt::graphs::TNodeID to_id,
904  bool ignoreCovariances = true) const
905  {
906  const typename BASE::edges_map_t::const_iterator itEdge =
907  BASE::edges.find(std::make_pair(from_id, to_id));
908  ASSERTMSG_(
909  itEdge != BASE::edges.end(),
910  format(
911  "Request for edge %u->%u that doesn't exist in graph.",
912  static_cast<unsigned int>(from_id),
913  static_cast<unsigned int>(to_id)));
914  return getEdgeSquareError(itEdge, ignoreCovariances);
915  }
916 
917  /** Empty all edges, nodes and set root to ID 0. */
918  inline void clear()
919  {
920  BASE::edges.clear();
921  nodes.clear();
922  root = 0;
924  }
925 
926  /** Return number of nodes in the list \a nodes of global coordinates
927  * (may be different that all nodes appearing in edges)
928  *
929  * \sa mrpt::graphs::CDirectedGraph::countDifferentNodesInEdges
930  */
931  inline size_t nodeCount() const { return nodes.size(); }
932  /** @} */
933 
934  private:
935  /**\brief Add a virtual edge between two nodes in the given graph.
936  *
937  * Edge is called virtual as its value will be determined solely on the
938  * pose difference of the given nodeIDs
939  */
940  inline static void addVirtualEdge(
941  self_t* graph, const TNodeID& from, const TNodeID& to)
942  {
943  ASSERTMSG_(
944  graph, "Invalid pointer to the graph instance was provided.");
945 
946  typename self_t::global_pose_t& p_from = graph->nodes.at(from);
947  typename self_t::global_pose_t& p_to = graph->nodes.at(to);
948  const typename BASE::edge_t& virt_edge(p_to - p_from);
949 
950  graph->insertEdge(from, to, virt_edge);
951  }
952 };
953 
954 /** Binary serialization (write) operator "stream << graph" */
955 template <
956  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
957  class EDGE_ANNOTATIONS>
960  const CNetworkOfPoses<
961  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>& obj)
962 {
963  using graph_t = CNetworkOfPoses<
964  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
966  return out;
967 }
968 
969 /** Binary serialization (read) operator "stream >> graph" */
970 template <
971  class CPOSE, class MAPS_IMPLEMENTATION, class NODE_ANNOTATIONS,
972  class EDGE_ANNOTATIONS>
976  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>& obj)
977 {
978  using graph_t = CNetworkOfPoses<
979  CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS>;
981  return in;
982 }
983 
984 /** \addtogroup mrpt_graphs_grp
985  * \name Handy typedefs for CNetworkOfPoses commonly used types
986  @{ */
987 
988 /** The specialization of CNetworkOfPoses for poses of type CPose2D (not a
989  * PDF!), also implementing serialization. */
991 /** The specialization of CNetworkOfPoses for poses of type mrpt::poses::CPose3D
992  * (not a PDF!), also implementing serialization. */
994 /** The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian,
995  * also implementing serialization. */
997 /** The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian,
998  * also implementing serialization. */
1000 /** The specialization of CNetworkOfPoses for poses of type CPosePDFGaussianInf,
1001  * also implementing serialization. */
1003 /** The specialization of CNetworkOfPoses for poses of type
1004  * CPose3DPDFGaussianInf, also implementing serialization. */
1005 using CNetworkOfPoses3DInf =
1007 
1008 /**\brief Specializations of CNetworkOfPoses for graphs whose nodes inherit from
1009  * TMRSlamNodeAnnotations struct */
1010 /**\{ */
1017 /**\} */
1018 
1019 /** @} */ // end of grouping
1020 
1021 } // namespace graphs
1022 
1023 // Specialization of TTypeName must occur in the same namespace:
1024 // These ones are here instead of into mrpt-containers, to avoid
1025 // the dependency mrpt-containers -> mrpt-typemeta
1026 namespace typemeta
1027 {
1030 } // namespace typemeta
1031 
1032 } // namespace mrpt
1033 
1034 // Implementation of templates (in a separate file for clarity)
1035 #include "CNetworkOfPoses_impl.h"
1036 
1037 // Visualization related template classes
1038 #include <mrpt/graphs/CMRVisualizer.h>
1039 #include <mrpt/graphs/CVisualizer.h>
A directed graph with the argument of the template specifying the type of the annotations in the edge...
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::serialization::CArchive &out)
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > &obj)
Binary serialization (read) operator "stream >> graph".
double getGlobalSquareError(bool ignoreCovariances=true) const
Evaluates the graph total square error (ignoreCovariances=true) or chi2 (ignoreCovariances=false) fro...
#define MRPT_START
Definition: exceptions.h:241
typename MAPS_IMPLEMENTATION::template map< mrpt::graphs::TNodeID, CPOSE > global_poses_pdf_t
A map from pose IDs to their global coordinate estimates, with uncertainty.
void clear()
Empty all edges, nodes and set root to ID 0.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::graphs::TNodeID root
The ID of the node that is the origin of coordinates, used as reference by all coordinates in nodes...
void dijkstra_nodes_estimate(std::optional< std::reference_wrapper< std::map< TNodeID, size_t >>> topological_distances=std::nullopt)
Spanning tree computation of a simple estimation of the global coordinates of each node just from the...
size_t collapseDuplicatedEdges()
Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them...
edges_map_t edges
The public member with the directed edges in the graph.
EDGE_ANNOTATIONS edge_annotations_t
The extra annotations in edges, apart from a constraint_t.
void saveToTextFile(const std::string &fileName) const
Saves to a text file in the format used by TORO, HoG-man, G2O.
friend std::ostream & operator<<(std::ostream &o, const self_t &global_pose)
Abstract graph and tree data structures, plus generic graph algorithms.
static constexpr auto getClassName()
void getNeighborsOf(const TNodeID nodeID, std::set< TNodeID > &neighborIDs) const
Return the list of all neighbors of "nodeID", by creating a list of their node IDs.
void loadFromTextFile(const std::string &fileName, bool collapse_dup_edges=true)
Loads from a text file in the format used by TORO & HoG-man (more on the format here) Recognized line...
CPOSE constraint_t
The type of PDF poses in the contraints (edges) (=CPOSE template argument)
double chi2() const
Returns the total chi-squared error of the graph.
STL namespace.
void getUnconnectedNodeIDs(std::set< mrpt::graphs::TNodeID > *set_nodeIDs) const
Fill set with the nodeIDs Dijkstra algorithm could not reach starting from the root node...
Definition: dijkstra.h:46
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::serialization::CArchive &in)
double getEdgeSquareError(const typename BASE::edges_map_t::const_iterator &itEdge, bool ignoreCovariances=true) const
Computes the square error of one pose constraints (edge) with respect to the global poses in nodes If...
void clearEdges()
Erase all edges.
A template to obtain the type of its argument as a string at compile time.
Definition: TTypeName.h:69
double getEdgeSquareError(const mrpt::graphs::TNodeID from_id, const mrpt::graphs::TNodeID to_id, bool ignoreCovariances=true) const
Computes the square error of one pose constraints (edge) with respect to the global poses in nodes If...
Base class for C*Visualizer classes.
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
Internal functions for MRPT.
static void save_graph_of_poses_to_ostream(const graph_t *g, std::ostream &f)
This base provides a set of functions for maths stuff.
typename CPOSE::type_value constraint_no_pdf_t
The type of edges or their means if they are PDFs (that is, a simple "edge" value) ...
constexpr auto literal(const char(&lit)[N_PLUS_1]) -> string_literal< N_PLUS_1 - 1 >
Definition: static_string.h:46
The type of each global pose in nodes: an extension of the constraint_no_pdf_t pose with any optional...
void insertEdge(TNodeID from_nodeID, TNodeID to_nodeID, const edge_t &edge_value)
Insert an edge (from -> to) with the given edge value.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr object, const mrpt::system::TParametersDouble &viz_params) const
Return 3D Visual Representation of the edges and nodes in the network of poses.
typename MAPS_IMPLEMENTATION::template map< mrpt::graphs::TNodeID, global_pose_t > global_poses_t
A map from pose IDs to their global coordinate estimates, without uncertainty (the "most-likely value...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
void mergeGraph(const self_t &other, const typename std::vector< detail::THypothesis< self_t >> &common_hypots, const bool hypots_from_other_to_self=true, std::map< TNodeID, TNodeID > *old_to_new_nodeID_mappings_out=nullptr)
Integrate given graph into own graph using the list of provided common THypotheses.
bool operator!=(const global_pose_t &other) const
static void connectGraphPartitions(self_t *sub_graph, const std::set< TNodeID > &groupA, const std::set< TNodeID > &groupB)
Add an edge between the last node of the group with the lower nodeIDs and the first node of the highe...
void writeAsText(std::ostream &o) const
Writes as text in the format used by TORO, HoG-man, G2O.
static void load_graph_of_poses_from_text_stream(graph_t *g, std::istream &f, const std::string &fil=std::string("(none)"))
size_t nodeCount() const
Return number of nodes in the list nodes of global coordinates (may be different that all nodes appea...
Traits for using a std::map<> (sparse representation)
Definition: traits_map.h:24
static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
void extractSubGraph(const std::set< TNodeID > &node_IDs, self_t *sub_graph, const TNodeID root_node_in=INVALID_NODEID, bool auto_expand_set=true) const
Find the edges between the nodes in the node_IDs set and fill given graph pointer accordingly...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
Traits for using a mrpt::containers::map_as_vector<> (dense, fastest representation) ...
Definition: traits_map.h:32
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
global_pose_t()=default
Potential class constructors.
mrpt::vision::TStereoCalibResults out
static double graph_edge_sqerror(const graph_t *g, const typename mrpt::graphs::CDirectedGraph< typename graph_t::constraint_t >::edges_map_t::const_iterator &itEdge, bool ignoreCovariances)
a helper struct with static template functions
#define MRPT_END
Definition: exceptions.h:245
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
void readAsText(std::istream &i)
Reads as text in the format used by TORO, HoG-man, G2O.
typename edges_map_t::const_iterator const_iterator
uint64_t TNodeID
A generic numeric type for unique IDs of nodes or entities.
Definition: TNodeID.h:16
static void graph_of_poses_dijkstra_init(graph_t *g, std::map< TNodeID, size_t > *topological_distances=nullptr)
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
MAPS_IMPLEMENTATION maps_implementation_t
The type of map&#39;s implementation (=MAPS_IMPLEMENTATION template argument)
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position...
global_pose_t(const ARG1 &a1, const ARG2 &a2)
The Dijkstra algorithm for finding the shortest path between a given source node in a (weighted) dire...
Definition: dijkstra.h:97
#define INVALID_NODEID
Definition: TNodeID.h:19
T absDiff(const T &lhs, const T &rhs)
Absolute difference between two numbers.
bool edges_store_inverse_poses
False (default) if an edge i->j stores the normal relative pose of j as seen from i: True if an edge...
bool operator==(const global_pose_t &other) const
An edge hypothesis between two nodeIDs.
Definition: THypothesis.h:32
Struct to be used as the NODE_ANNOTATIONS template argument in CNetworkOfPoses class instances for us...
Struct to be used as the NODE_ANNOTATIONS template argument in CNetworkOfPoses class instances for us...
Wrapper class that provides visualization of a network of poses that have been registered by many gra...
Definition: CMRVisualizer.h:36
Custom exception class that passes information in case an unconnected graph is passed to a Dijkstra i...
Definition: dijkstra.h:30
NODE_ANNOTATIONS node_annotations_t
The extra annotations in nodes, apart from a constraint_no_pdf_t.
typename CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS >::global_pose_t self_t
#define MRPT_DECLARE_TTYPENAME(_TYPE)
Definition: TTypeName.h:111
static void addVirtualEdge(self_t *graph, const TNodeID &from, const TNodeID &to)
Add a virtual edge between two nodes in the given graph.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020