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