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