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