Main MRPT website > C++ reference for MRPT 1.5.6
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 file methods
191  @{ */
192 
193  /** Saves to a text file in the format used by TORO & HoG-man (more on
194  * the format <a href="http://www.mrpt.org/Robotics_file_formats" * >here</a>)
195  * For 2D graphs only VERTEX2 & EDGE2 entries will be saved,
196  * and VERTEX3 & EDGE3 entries for 3D graphs. Note that EQUIV entries
197  * will not be saved, but instead several EDGEs will be stored between
198  * the same node IDs.
199  *
200  * \sa saveToBinaryFile, loadFromTextFile
201  * \exception On any error
202  */
203  inline void saveToTextFile(const std::string &fileName) const {
205  }
206 
207  /** 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>)
208  * Recognized line entries are: VERTEX2, VERTEX3, EDGE2, EDGE3, EQUIV.
209  * If an unknown entry is found, a warning is dumped to std::cerr (only once for each unknown keyword).
210  * 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).
211  *
212  * \param[in] fileName The file to load.
213  * \param[in] collapse_dup_edges If true, \a collapseDuplicatedEdges will be
214  * called automatically after loading (note that this operation may take
215  * significant time for very large graphs).
216  *
217  * \sa loadFromBinaryFile, saveToTextFile
218  *
219  * \exception On any error, as a malformed line or loading a 3D graph in
220  * a 2D graph.
221  */
222  inline void loadFromTextFile(const std::string &fileName, bool collapse_dup_edges = true) {
224  if (collapse_dup_edges) this->collapseDuplicatedEdges();
225  }
226 
227  /** @} */
228 
229  /** @name Utility methods
230  @{ */
231  /**\brief Return 3D Visual Representation of the edges and nodes in the
232  * network of poses
233  *
234  * Method makes the call to the corresponding method of the CVisualizer
235  * class instance.
236  */
238  mrpt::opengl::CSetOfObjectsPtr object,
239  const mrpt::utils::TParametersDouble& viz_params) const
240  {
243 
244  bool is_multirobot=false;
245 #if MRPT_HAS_CXX11
246  std::unique_ptr<visualizer_t> viz;
248 #else
249  std::auto_ptr<visualizer_t> viz;
250  // Initialize instance of class visualizer
252  is_multirobot = dynamic_cast<mrpt::graphs::detail::TMRSlamNodeAnnotations*>(&n)!=NULL;
253 #endif
254  if (is_multirobot) {
255  viz.reset(new visualizer_multirobot_t(*this));
256  }
257  else {
258  viz.reset(new visualizer_t(*this));
259  }
260  viz->getAs3DObject(object, viz_params);
261  }
262 
263  /** Spanning tree computation of a simple estimation of the global
264  * coordinates of each node just from the information in all edges,
265  * sorted in a Dijkstra tree based on the current "root" node.
266  *
267  * \note The "global" coordinates are with respect to the node with the
268  * ID specified in \a root.
269  *
270  * \note This method takes into account the
271  * value of \a edges_store_inverse_poses
272  *
273  * \sa node, root
274  */
276 
277  /** Look for duplicated edges (even in opposite directions) between all
278  * pairs of nodes and fuse them. Upon return, only one edge remains
279  * between each pair of nodes with the mean & covariance (or information
280  * matrix) corresponding to the Bayesian fusion of all the Gaussians.
281  *
282  * \return Overall number of removed edges.
283  */
285 
286  /** Computes the overall square error from all the pose constraints (edges) with respect to the global poses in \a nodes
287  * If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
288  * \sa getEdgeSquareError
289  * \exception std::exception On global poses not in \a nodes
290  */
291  double getGlobalSquareError(bool ignoreCovariances = true) const {
292  double sqErr=0;
293  const typename BASE::edges_map_t::const_iterator last_it=BASE::edges.end();
294  for (typename BASE::edges_map_t::const_iterator itEdge=BASE::edges.begin();itEdge!=last_it;++itEdge)
295  sqErr+=detail::graph_ops<self_t>::graph_edge_sqerror(this,itEdge,ignoreCovariances);
296  return sqErr;
297  }
298 
299  /**\brief Find the edges between the nodes in the node_IDs set and fill
300  * given graph pointer accordingly.
301  *
302  * \param[in] node_IDs Set of nodes, between which, edges should be found
303  * and inserted in the given sub_graph pointer
304  * \param[in] root_node_in Node ID to be used as the root node of
305  * sub_graph. If this is not given, the lowest nodeID is to be used.
306  * \param[out] CNetworkOfPoses pointer that is to be filled.
307  *
308  * \param[in] auto_expand_set If true and in case the node_IDs set
309  * contains non-consecutive nodes the returned set is expanded with the
310  * in-between nodes. This makes sure that the final graph is always
311  * connected.
312  * If auto_expand_set is false but there exist
313  * non-consecutive nodes, virtual edges are inserted in the parts that
314  * the graph is not connected
315  */
316  void extractSubGraph(const std::set<TNodeID>& node_IDs,
317  self_t* sub_graph,
318  const TNodeID root_node_in=INVALID_NODEID,
319  const bool& auto_expand_set=true) const {
320  using namespace std;
321  using namespace mrpt;
322  using namespace mrpt::math;
323  using namespace mrpt::graphs::detail;
324 
325  typedef CDijkstra<self_t, MAPS_IMPLEMENTATION> dijkstra_t;
326 
327  // assert that the given pointers are valid
328  ASSERTMSG_(sub_graph,
329  "\nInvalid pointer to a CNetworkOfPoses instance is given. Exiting..\n");
330  sub_graph->clear();
331 
332  // assert that the root_node actually exists in the given node_IDs set
333  TNodeID root_node = root_node_in;
334  if (root_node != INVALID_NODEID) {
335  ASSERTMSG_(node_IDs.find(root_node) != node_IDs.end(),
336  "\nRoot_node does not exist in the given node_IDs set. Exiting.\n");
337  }
338 
339  // ask for at least 2 nodes
340  ASSERTMSG_(node_IDs.size() >= 2,
341  format(
342  "Very few nodes [%lu] for which to extract a subgraph. Exiting\n",
343  static_cast<unsigned long>(node_IDs.size())));
344 
345 
346  // find out if querry contains non-consecutive nodes.
347  // Assumption: Set elements are in standard, ascending order.
348  bool is_fully_connected_graph = true;
349  std::set<TNodeID> node_IDs_real; // actual set of nodes to be used.
350  if (*node_IDs.rbegin() - *node_IDs.begin() + 1 == node_IDs.size()) {
351  node_IDs_real = node_IDs;
352  }
353  else { // contains non-consecutive nodes
354  is_fully_connected_graph = false;
355 
356  if (auto_expand_set) { // set auto-expansion
357  for (TNodeID curr_node_ID = *node_IDs.begin();
358  curr_node_ID != *node_IDs.rbegin(); ++curr_node_ID) {
359  node_IDs_real.insert(curr_node_ID);
360  }
361  }
362  else { // virtual_edge_addition strategy
363  node_IDs_real = node_IDs;
364  }
365  }
366 
367  // add all the nodes of the node_IDs_real set to sub_graph
369  node_IDs_it = node_IDs_real.begin();
370  node_IDs_it != node_IDs_real.end();
371  ++node_IDs_it) {
372 
373  // assert that current node exists in *own* graph
374  typename global_poses_t::const_iterator own_it;
375  for (own_it = nodes.begin(); own_it != nodes.end(); ++own_it) {
376  if (*node_IDs_it == own_it->first) {
377  break; // I throw exception afterwards
378  }
379  }
380  ASSERTMSG_(own_it != nodes.end(),
381  format("NodeID [%lu] can't be found in the initial graph.",
382  static_cast<unsigned long>(*node_IDs_it)));
383 
384  global_pose_t curr_node(nodes.at(*node_IDs_it));
385  sub_graph->nodes.insert(make_pair(*node_IDs_it, curr_node));
386 
387  }
388  //cout << "Extracting subgraph for nodeIDs: " <<
389  //getSTLContainerAsString(node_IDs_real) << endl;
390 
391  // set the root of the extracted graph
392  if (root_node == INVALID_NODEID) {
393  // smallest nodeID by default
394  // http://stackoverflow.com/questions/1342045/how-do-i-find-the-largest-int-in-a-stdsetint
395  // std::set sorts elements in ascending order
396  root_node = sub_graph->nodes.begin()->first;
397  }
398  sub_graph->root = root_node;
399 
400  // TODO - Remove these lines - not needed
401  // set the corresponding root pose
402  //sub_graph->nodes.at(sub_graph->root) = nodes.at(sub_graph->root);
403 
404  // find all edges (in the initial graph), that exist in the given set
405  // of nodes; add them to the given graph
406  sub_graph->clearEdges();
407  for (typename BASE::const_iterator it = BASE::edges.begin();
408  it != BASE::edges.end();
409  ++it) {
410 
411  const TNodeID& from = it->first.first;
412  const TNodeID& to = it->first.second;
413  const typename BASE::edge_t& curr_edge = it->second;
414 
415  // if both nodes exist in the given set, add the corresponding edge
416  if (sub_graph->nodes.find(from) != sub_graph->nodes.end() &&
417  sub_graph->nodes.find(to) != sub_graph->nodes.end()) {
418  sub_graph->insertEdge(from, to, curr_edge);
419  }
420  }
421 
422  if (!auto_expand_set && !is_fully_connected_graph) {
423  // Addition of virtual edges between non-connected graph parts is necessary
424 
425  // make sure that the root nodeID is connected to at least one node
426  {
427  std::set<TNodeID> root_neighbors;
428  sub_graph->getNeighborsOf(sub_graph->root, root_neighbors);
429 
430  if (root_neighbors.empty()) {
431  // add an edge between the root and an adjacent nodeID
432  typename global_poses_t::iterator root_it =
433  sub_graph->nodes.find(sub_graph->root);
434  ASSERT_(root_it != sub_graph->nodes.end());
435  if ((*root_it == *sub_graph->nodes.rbegin())) { // is the last nodeID
436  // add with previous node
437  TNodeID next_to_root = (--root_it)->first;
438  self_t::addVirtualEdge(sub_graph, next_to_root, sub_graph->root);
439  //cout << "next_to_root = " << next_to_root;
440  }
441  else {
442  TNodeID next_to_root = (++root_it)->first;
443  //cout << "next_to_root = " << next_to_root;
444  self_t::addVirtualEdge(sub_graph, sub_graph->root, next_to_root);
445  }
446 
447  }
448  }
449 
450 
451  // as long as the graph is unconnected (as indicated by Dijkstra) add a virtual edge between
452  bool dijkstra_runs_successfully = false;
453 
454  // loop until the graph is fully connected (i.e. I can reach every
455  // node of the graph starting from its root)
456  while (!dijkstra_runs_successfully) {
457  try {
458  dijkstra_t dijkstra(*sub_graph, sub_graph->root);
459  dijkstra_runs_successfully = true;
460  }
461  catch (const mrpt::graphs::detail::NotConnectedGraph& ex) {
462  dijkstra_runs_successfully = false;
463 
464  set<TNodeID> unconnected_nodeIDs;
465  ex.getUnconnectedNodeIDs(&unconnected_nodeIDs);
466  //cout << "Unconnected nodeIDs: " << mrpt::math::getSTLContainerAsString(unconnected_nodeIDs) << endl;
467  // mainland: set of nodes that the root nodeID is in
468  // island: set of nodes that the Dijkstra graph traversal can't
469  // reach starting from the root.
470  // [!] There may be multiple sets of these nodes
471 
472  // set::rend() is the element with the highest value
473  // set::begin() is the element with the lowest value
474  const TNodeID& island_highest = *unconnected_nodeIDs.rbegin();
475  const TNodeID& island_lowest = *unconnected_nodeIDs.begin();
476  //cout << "island_highest: " << island_highest << endl;
477  //cout << "island_lowest: " << island_lowest << endl;
478  //cout << "root: " << sub_graph->root << endl;
479 
480  // find out which nodes are in the same partition with the root
481  // (i.e. mainland)
482  std::set<TNodeID> mainland;
483  // for all nodes in sub_graph
484  for (typename global_poses_t::const_iterator
485  n_it = sub_graph->nodes.begin();
486  n_it != sub_graph->nodes.end();
487  ++n_it) {
488  bool is_there = false;
489 
490  // for all unconnected nodes
492  uncon_it = unconnected_nodeIDs.begin();
493  uncon_it != unconnected_nodeIDs.end();
494  ++uncon_it) {
495 
496  if (n_it->first == *uncon_it) {
497  is_there = true;
498  break;
499  }
500  }
501 
502  if (!is_there) {
503  mainland.insert(n_it->first);
504  }
505  }
506 
507  bool is_single_island = (island_highest - island_lowest + 1 ==
508  unconnected_nodeIDs.size());
509 
510  if (is_single_island) { // single island
511  // Possible scenarios:
512  // | island | | mainland |
513  // | <low nodeIDs> island_highest| --- <virtual_edge> --->> | mainland_lowest <high nodeIDs> ... root ...|
514  // --- OR ---
515  // | mainland | | island |
516  // | <low nodeIDs> mainland_highest| --- <virtual_edge> --->> | island_lowest <high nodeIDs> |
517 
518  const std::set<TNodeID>& island = unconnected_nodeIDs;
519  this->connectGraphPartitions(sub_graph, island, mainland);
520 
521  }
522  else { // multiple islands
523  // add a virtual edge between the last group before the mainland and the mainland
524 
525  // split the unconnected_nodeIDs to smaller groups of nodes
526  // we only care about the nodes that are prior to the root
527  std::vector<std::set<TNodeID> > vec_of_islands;
528  std::set<TNodeID> curr_island;
529  TNodeID prev_nodeID = *unconnected_nodeIDs.begin();
530  curr_island.insert(prev_nodeID); // add the initial node;
532  it = ++unconnected_nodeIDs.begin();
533  *it < sub_graph->root && it != unconnected_nodeIDs.end();
534  ++it) {
535  if (!(absDiff(*it, prev_nodeID) == 1)) {
536  vec_of_islands.push_back(curr_island);
537  curr_island.clear();
538  }
539  curr_island.insert(*it);
540 
541  // update the previous nodeID
542  prev_nodeID = *it;
543  }
544  vec_of_islands.push_back(curr_island);
545 
546  //cout << "last_island: " << getSTLContainerAsString(vec_of_islands.back()) << endl;
547  //cout << "mainland: " << getSTLContainerAsString(mainland) << endl;
548  this->connectGraphPartitions(sub_graph, vec_of_islands.back(), mainland);
549  }
550  }
551  }
552  }
553 
554  // estimate the node positions according to the edges - root is (0, 0, 0)
555  // just execute dijkstra once for grabbing the updated node positions.
556  sub_graph->dijkstra_nodes_estimate();
557 
558  } // end of extractSubGraph
559 
560  /**\brief Integrate given graph into own graph using the list of provided
561  * common THypotheses. Nodes of the other graph are renumbered upon
562  * integration in own graph.
563  *
564  * \param[in] other Graph (of the same type) that is to be integrated with own graph.
565  * \param[in] Hypotheses that join own and other graph.
566  * \param[in] hypots_from_other_to_self Specify the direction of the
567  * THypothesis objects in the common_hypots. If true (default) they are
568  * directed from other to own graph (other \rightarrow own),
569  *
570  * \param[out] old_to_new_nodeID_mappings_out Map from the old nodeIDs
571  * that are in the given graph to the new nodeIDs that have been inserted
572  * (by this method) in own graph.
573  */
574  inline void mergeGraph(
575  const self_t& other,
576  const typename std::vector<detail::THypothesis<self_t> >& common_hypots,
577  const bool hypots_from_other_to_self=true,
578  std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings_out=NULL) {
579  MRPT_START;
580  using namespace mrpt::graphs;
581  using namespace mrpt::utils;
582  using namespace mrpt::graphs::detail;
583  using namespace std;
584 
585  typedef typename vector<THypothesis<self_t> >::const_iterator hypots_cit_t;
586  typedef typename global_poses_t::const_iterator nodes_cit_t;
587 
588  const self_t& graph_from = (hypots_from_other_to_self? other : *this);
589  const self_t& graph_to = (hypots_from_other_to_self? *this : other);
590 
591  // assert that both own and other graph have at least two nodes.
592  ASSERT_(graph_from.nodes.size() >= 2);
593  ASSERT_(graph_to.nodes.size() >= 2);
594 
595  // Assert that from-nodeIds, to-nodeIDs in common_hypots exist in own
596  // and other graph respectively
597  for (hypots_cit_t h_cit = common_hypots.begin();
598  h_cit != common_hypots.end();
599  ++h_cit) {
600 
601  ASSERTMSG_(graph_from.nodes.find(h_cit->from) != graph_from.nodes.end(),
602  format("NodeID %lu is not found in (from) graph", h_cit->from))
603  ASSERTMSG_(graph_to.nodes.find(h_cit->to) != graph_to.nodes.end(),
604  format("NodeID %lu is not found in (to) graph", h_cit->to))
605  }
606 
607  // find the max nodeID in existing graph
608  TNodeID max_nodeID = 0;
609  for (nodes_cit_t n_cit = this->nodes.begin();
610  n_cit != this->nodes.end();
611  ++n_cit) {
612  if (n_cit->first > max_nodeID) {
613  max_nodeID = n_cit->first;
614  }
615  }
616  TNodeID renum_start = max_nodeID + 1;
617  size_t renum_counter = 0;
618  //cout << "renum_start: " << renum_start << endl;
619 
620  // Renumber nodeIDs of other graph so that they don't overlap with own
621  // graph nodeIDs
622  std::map<TNodeID, TNodeID>* old_to_new_nodeID_mappings;
623 
624  // map of TNodeID->TNodeID correspondences to address to if the
625  // old_to_new_nodeID_mappings_out is not given.
626  // Handy for not having to allocate old_to_new_nodeID_mappings in the
627  // heap
628  std::map<TNodeID, TNodeID> mappings_tmp;
629 
630  // If given, use the old_to_new_nodeID_mappings map.
631  if (old_to_new_nodeID_mappings_out) {
632  old_to_new_nodeID_mappings = old_to_new_nodeID_mappings_out;
633  }
634  else {
635  old_to_new_nodeID_mappings = &mappings_tmp;
636  }
637  old_to_new_nodeID_mappings->clear();
638 
639  // add all nodes of other graph - Take care of renumbering them
640  //cout << "Adding nodes of other graph" << endl;
641  //cout << "====================" << endl;
642  for (nodes_cit_t n_cit = other.nodes.begin();
643  n_cit != other.nodes.end();
644  ++n_cit) {
645  TNodeID new_nodeID = renum_start + renum_counter++;
646  old_to_new_nodeID_mappings->insert(make_pair(
647  n_cit->first,
648  new_nodeID));
649  this->nodes.insert(make_pair(
650  new_nodeID, n_cit->second));
651 
652  //cout << "Adding nodeID: " << new_nodeID << endl;
653  }
654 
655  // add common constraints
656  //cout << "Adding common constraints" << endl;
657  //cout << "====================" << endl;
658  for (hypots_cit_t h_cit = common_hypots.begin();
659  h_cit != common_hypots.end();
660  ++h_cit) {
661  TNodeID from, to;
662  if (hypots_from_other_to_self) {
663  from = old_to_new_nodeID_mappings->at(h_cit->from);
664  to = h_cit->to;
665  }
666  else {
667  from = h_cit->from;
668  to = old_to_new_nodeID_mappings->at(h_cit->to);
669  }
670  this->insertEdge(from, to, h_cit->getEdge());
671  //cout << from << " -> " << to << " => " << h_cit->getEdge() << endl;
672  }
673 
674  // add all constraints of the other graph
675  //cout << "Adding constraints of other graph" << endl;
676  //cout << "====================" << endl;
677  for (typename self_t::const_iterator
678  g_cit = other.begin();
679  g_cit != other.end();
680  ++g_cit) {
681  TNodeID new_from = old_to_new_nodeID_mappings->at(g_cit->first.first);
682  TNodeID new_to = old_to_new_nodeID_mappings->at(g_cit->first.second);
683  this->insertEdge(new_from, new_to, g_cit->second);
684 
685  //cout << "[" << new_from << "] -> [" << new_to << "]" << " => " << g_cit->second << endl;
686  }
687 
688  // run Dijkstra to update the node positions
689  this->dijkstra_nodes_estimate();
690 
691  MRPT_END;
692  }
693 
694  /**\brief Add an edge between the last node of the group with the lower
695  * nodeIDs and the first node of the higher nodeIDs.
696  *
697  * Given groups of nodes should only contain consecutive nodeIDs and
698  * there should be no overlapping between them
699  *
700  * \note It is assumed that the sets of nodes are \b already in ascending
701  * order (default std::set behavior.
702  */
703  inline static void connectGraphPartitions(
704  self_t* sub_graph,
705  const std::set<TNodeID>& groupA,
706  const std::set<TNodeID>& groupB) {
707  using namespace mrpt::math;
708 
709  ASSERTMSG_(sub_graph,
710  "\nInvalid pointer to a CNetworkOfPoses instance is given. Exiting..\n");
711  ASSERTMSG_(!groupA.empty(), "\ngroupA is empty.");
712  ASSERTMSG_(!groupB.empty(), "\ngroupB is empty.");
713 
714  // assertion - non-overlapping groups
715  ASSERTMSG_(
716  *groupA.rend() < *groupB.rbegin() ||
717  *groupA.rbegin() > *groupB.rend(),
718  "Groups A, B contain overlapping nodeIDs");
719 
720  // decide what group contains the low/high nodeIDs
721  // just compare any two nodes of the sets (they are non-overlapping
722  const std::set<TNodeID>& low_nodeIDs =
723  *groupA.rbegin() < *groupB.rbegin() ? groupA : groupB;
724  const std::set<TNodeID>& high_nodeIDs =
725  *groupA.rbegin() > *groupB.rbegin() ? groupA : groupB;
726 
727  // add virtual edge
728  const TNodeID& from_nodeID = *low_nodeIDs.rbegin();
729  const TNodeID& to_nodeID = *high_nodeIDs.begin();
730  self_t::addVirtualEdge(sub_graph, from_nodeID, to_nodeID);
731 
732  }
733 
734  /** Computes the square error of one pose constraints (edge) with respect
735  * to the global poses in \a nodes If \a ignoreCovariances is false, the
736  * squared Mahalanobis distance will be computed instead of the straight
737  * square error.
738  *
739  * \exception std::exception On global poses not in \a nodes
740  */
741  inline double getEdgeSquareError(
742  const typename BASE::edges_map_t::const_iterator &itEdge,
743  bool ignoreCovariances = true) const {
744 
746  this,
747  itEdge,
748  ignoreCovariances);
749  }
750 
751  /** Computes the square error of one pose constraints (edge) with respect
752  * to the global poses in \a nodes If \a ignoreCovariances is false, the
753  * squared Mahalanobis distance will be computed instead of the straight
754  * square error.
755  *
756  * \exception std::exception On edge not existing or global poses not in
757  * \a nodes
758  */
759  double getEdgeSquareError(const mrpt::utils::TNodeID from_id, const mrpt::utils::TNodeID to_id, bool ignoreCovariances = true) const
760  {
761  const typename BASE::edges_map_t::const_iterator itEdge = BASE::edges.find(std::make_pair(from_id,to_id));
762  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)));
763  return getEdgeSquareError(itEdge,ignoreCovariances);
764  }
765 
766  /** Empty all edges, nodes and set root to ID 0. */
767  inline void clear() {
768  BASE::edges.clear();
769  nodes.clear();
770  root = 0;
772  }
773 
774  /** Return number of nodes in the list \a nodes of global coordinates
775  * (may be different that all nodes appearing in edges)
776  *
777  * \sa mrpt::graphs::CDirectedGraph::countDifferentNodesInEdges
778  */
779  inline size_t nodeCount() const { return nodes.size(); }
780 
781  /** @} */
782 
783  /** @name Ctors & Dtors
784  @{ */
785 
786  /** Default constructor (just sets root to "0" and
787  * edges_store_inverse_poses to "false") */
788  inline CNetworkOfPoses():
789  root(0),
791  {
792  }
793  /** @} */
794 
795  private:
796  /**\brief Add a virtual edge between two nodes in the given graph.
797  *
798  * Edge is called virtual as its value will be determined solely on the
799  * pose difference of the given nodeIDs
800  */
801  inline static void addVirtualEdge(
802  self_t* graph,
803  const TNodeID& from,
804  const TNodeID& to) {
805  ASSERTMSG_(graph, "Invalid pointer to the graph instance was provided.");
806 
807  typename self_t::global_pose_t& p_from = graph->nodes.at(from);
808  typename self_t::global_pose_t& p_to = graph->nodes.at(to);
809  const typename BASE::edge_t& virt_edge(p_to - p_from);
810 
811  graph->insertEdge(from, to, virt_edge);
812  }
813 
814  }; // end class
815 
816  /** Binary serialization (write) operator "stream << graph" */
817  template <class CPOSE,class MAPS_IMPLEMENTATION,class NODE_ANNOTATIONS,class EDGE_ANNOTATIONS>
818  mrpt::utils::CStream & operator << (mrpt::utils::CStream&out, const CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION,NODE_ANNOTATIONS,EDGE_ANNOTATIONS> &obj)
819  {
822  return out;
823  }
824 
825  /** Binary serialization (read) operator "stream >> graph" */
826  template <class CPOSE,class MAPS_IMPLEMENTATION,class NODE_ANNOTATIONS,class EDGE_ANNOTATIONS>
828  {
831  return in;
832  }
833 
834  /** \addtogroup mrpt_graphs_grp
835  * \name Handy typedefs for CNetworkOfPoses commonly used types
836  @{ */
837 
838  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.
839  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.
840  typedef CNetworkOfPoses<mrpt::poses::CPosePDFGaussian, mrpt::utils::map_traits_stdmap> CNetworkOfPoses2DCov; //!< The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian, also implementing serialization.
841  typedef CNetworkOfPoses<mrpt::poses::CPose3DPDFGaussian, mrpt::utils::map_traits_stdmap> CNetworkOfPoses3DCov; //!< The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian, also implementing serialization.
842  typedef CNetworkOfPoses<mrpt::poses::CPosePDFGaussianInf, mrpt::utils::map_traits_stdmap> CNetworkOfPoses2DInf; //!< The specialization of CNetworkOfPoses for poses of type CPosePDFGaussianInf, also implementing serialization.
843  typedef CNetworkOfPoses<mrpt::poses::CPose3DPDFGaussianInf, mrpt::utils::map_traits_stdmap> CNetworkOfPoses3DInf; //!< The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussianInf, also implementing serialization.
844 
845  /**\brief Specializations of CNetworkOfPoses for graphs whose nodes inherit from TMRSlamNodeAnnotations struct */
846  /**\{ */
849  /**\} */
850 
851  /** @} */ // end of grouping
852 
853 
854  } // End of namespace
855 
856  // Specialization of TTypeName must occur in the same namespace:
857  namespace utils
858  {
859  // Extensions to mrpt::utils::TTypeName for matrices:
860  template<
861  class CPOSE,
862  class MAPS_IMPLEMENTATION,
863  class NODE_ANNOTATIONS,
864  class EDGE_ANNOTATIONS
865  >
866  struct TTypeName <mrpt::graphs::CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION,NODE_ANNOTATIONS,EDGE_ANNOTATIONS> >
867  {
868  static std::string get()
869  {
870  return std::string("mrpt::graphs::CNetworkOfPoses<")
875  +std::string(">");
876  }
877  };
878 
881 
882  }
883 
884 } // End of namespace
885 
886 
887 // Implementation of templates (in a separate file for clarity)
888 #include "CNetworkOfPoses_impl.h"
889 
890 // Visualization related template classes
891 #include <mrpt/graphs/CVisualizer.h>
893 
894 #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...
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 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...
static void graph_of_poses_dijkstra_init(graph_t *g)
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 nodeCount() const
Return number of nodes in the list nodes of global coordinates (may be different that all nodes appea...
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.
static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
friend std::ostream & operator<<(std::ostream &o, const self_t &global_pose)
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
bool operator==(const global_pose_t &other) const
#define INVALID_NODEID
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.
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)
const Scalar * const_iterator
Definition: eigen_plugins.h:24
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.
GLuint in
Definition: glew.h:7146
Base class for C*Visualizer classes.
A directed graph of pose constraints, with edges being the relative poses between pairs of nodes iden...
GLsizei const GLfloat * value
Definition: glew.h:1756
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
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.
GLsizei n
Definition: glew.h:5051
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...
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
GLhandleARB obj
Definition: glew.h:3276
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
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) ...
static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
GLint * first
Definition: glew.h:1433
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
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
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!)...
#define MRPT_START
bool operator!=(const global_pose_t &other) const
GLsizei const GLcharARB ** string
Definition: glew.h:3293
CPOSE constraint_t
The type of PDF poses in the contraints (edges) (=CPOSE template argument)
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, CPOSE > global_poses_pdf_t
A map from pose IDs to their global coordinate estimates, with uncertainty.
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)
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...
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...
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
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.
EDGE_ANNOTATIONS edge_annotations_t
The extra annotations in edges, apart from a constraint_t.
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...
double getGlobalSquareError(bool ignoreCovariances=true) const
Computes the overall square error from all the pose constraints (edges) with respect to the global po...
#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
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...
MAPS_IMPLEMENTATION maps_implementation_t
The type of map'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.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018