Main MRPT website > C++ reference
MRPT logo
CNetworkOfPoses.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CONSTRAINED_POSE_NETWORK_H
29 #define CONSTRAINED_POSE_NETWORK_H
30 
31 #include <mrpt/poses/CPosePDFGaussian.h>
32 #include <mrpt/poses/CPose3DPDFGaussian.h>
33 #include <mrpt/poses/CPosePDFGaussianInf.h>
34 #include <mrpt/poses/CPose3DPDFGaussianInf.h>
35 #include <mrpt/graphs/CDirectedGraph.h>
36 #include <mrpt/graphs/CDirectedTree.h>
37 
38 #include <mrpt/utils/CSerializable.h>
39 #include <mrpt/utils/CFileGZInputStream.h>
40 #include <mrpt/utils/CFileGZOutputStream.h>
41 
42 #include <mrpt/utils/traits_map.h>
43 #include <mrpt/utils/stl_extensions.h>
44 
45 #include <mrpt/graphs/link_pragmas.h>
46 
47 // The main class in this file is CNetworkOfPoses, a generic basic template for
48 // predefined 2D/3D graphs of pose contraints.
49 namespace mrpt
50 {
51  namespace graphs
52  {
53  using mrpt::utils::TNodeID;
54  using namespace mrpt::utils;
55 
56  /** Internal functions for MRPT */
57  namespace detail
58  {
59  template <class GRAPH_T> struct graph_ops;
60  /** An empty structure */
61  struct node_annotations_empty { };
62  }
63 
64  /** A directed graph of pose constraints, with edges being the relative pose between pairs of nodes indentified by their numeric IDs (of type TNodeID).
65  * 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".
66  * These poses are stored in the edges in the format specified by the template argument CPOSE. Users should employ the following derived classes
67  * depending on the desired representation of edges:
68  * - mrpt::graphs::CNetworkOfPoses2D : 2D edges as a simple CPose2D (x y phi)
69  * - mrpt::graphs::CNetworkOfPoses3D : 3D edges as a simple CPose3D (x y z yaw pitch roll)
70  * - mrpt::graphs::CNetworkOfPoses2DInf : 2D edges as a Gaussian PDF with information matrix ( CPosePDFGaussianInf )
71  * - mrpt::graphs::CNetworkOfPoses3DInf : 3D edges as a Gaussian PDF with information matrix ( CPose3DPDFGaussianInf )
72  * - mrpt::graphs::CNetworkOfPoses2DCov : 2D edges as a Gaussian PDF with covariance matrix ( CPosePDFGaussian ). It's more efficient to use the information matrix version instead!
73  * - mrpt::graphs::CNetworkOfPoses3DCov : 3D edges as a Gaussian PDF with covariance matrix ( CPose3DPDFGaussian ). It's more efficient to use the information matrix version instead!
74  *
75  * Two main members store all the information in this class:
76  * - \a edge (in the base class mrpt::graphs::CDirectedGraph::edge): A map from pairs of node ID -> pose constraints.
77  * - \a nodes : A map from node ID -> estimated pose of that node (actually, read below on the template argument MAPS_IMPLEMENTATION).
78  *
79  * 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> ),
80  * using \a loadFromTextFile and \a saveToTextFile.
81  *
82  * This class is the base for representing networks of poses, which are the main data type of a series
83  * of SLAM algorithms implemented in the library mrpt-slam, in the namespace mrpt::graphslam.
84  *
85  * For tools to visualize graphs as 2D/3D plots, see the namespace mrpt::opengl::graph_tools in the library mrpt-opengl.
86  *
87  * The template arguments are:
88  * - CPOSE: The type of the edges, which hold a relative pose (2D/3D, just a value or a Gaussian, etc.)
89  * - MAPS_IMPLEMENTATION: Can be either mrpt::utils::map_traits_stdmap or mrpt::utils::map_traits_map_as_vector. Determines the type of the list of global poses (member \a nodes).
90  *
91  * \sa mrpt::graphslam
92  * \ingroup mrpt_graphs_grp
93  */
94  template<
95  class CPOSE, // Type of edges
96  class MAPS_IMPLEMENTATION = map_traits_stdmap, // Use std::map<> vs. std::vector<>
97  class NODE_ANNOTATIONS = mrpt::graphs::detail::node_annotations_empty,
98  class EDGE_ANNOTATIONS = mrpt::graphs::detail::edge_annotations_empty
99  >
100  class CNetworkOfPoses : public mrpt::graphs::CDirectedGraph< CPOSE, EDGE_ANNOTATIONS >
101  {
102  public:
103  /** @name Typedef's
104  @{ */
105  typedef mrpt::graphs::CDirectedGraph<CPOSE,EDGE_ANNOTATIONS> BASE; //!< The base class "CDirectedGraph<CPOSE,EDGE_ANNOTATIONS>" */
106  typedef CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION,NODE_ANNOTATIONS,EDGE_ANNOTATIONS> self_t; //!< My own type
107 
108  typedef CPOSE constraint_t; //!< The type of PDF poses in the contraints (edges) (=CPOSE template argument)
109  typedef NODE_ANNOTATIONS node_annotations_t; //!< The extra annotations in nodes, apart from a \a constraint_no_pdf_t
110  typedef EDGE_ANNOTATIONS edge_annotations_t; //!< The extra annotations in edges, apart from a \a constraint_t
111 
112  typedef MAPS_IMPLEMENTATION maps_implementation_t; //!< The type of map's implementation (=MAPS_IMPLEMENTATION template argument)
113  typedef typename CPOSE::type_value constraint_no_pdf_t; //!< The type of edges or their means if they are PDFs (that is, a simple "edge" value)
114 
115  /** The type of each global pose in \a nodes: an extension of the \a constraint_no_pdf_t pose with any optional user-defined data */
116  struct global_pose_t : public constraint_no_pdf_t, public NODE_ANNOTATIONS
117  {
118  // Replicate possible constructors:
119  inline global_pose_t() : constraint_no_pdf_t() { }
120  template <typename ARG1> inline global_pose_t(const ARG1 &a1) : constraint_no_pdf_t(a1) { }
121  template <typename ARG1,typename ARG2> inline global_pose_t(const ARG1 &a1,const ARG2 &a2) : constraint_no_pdf_t(a1,a2) { }
122  };
123 
124  /** A map from pose IDs to their global coordinates estimates, with uncertainty */
125  typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,CPOSE> global_poses_pdf_t;
126 
127  /** A map from pose IDs to their global coordinates estimates, without uncertainty (the "most-likely value") */
128  typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,global_pose_t> global_poses_t;
129 
130  /** @} */
131 
132 
133  /** @name Data members
134  @{ */
135 
136  /** The nodes (vertices) of the graph, with their estimated "global" (with respect to \a root) position, without an associated covariance.
137  * \sa dijkstra_nodes_estimate
138  */
139  global_poses_t nodes;
140 
141  /** The ID of the node that is the origin of coordinates, used as reference by all coordinates in \nodes. By default, root is the ID "0". */
142  TNodeID root;
143 
144  /** False (default) if an edge i->j stores the normal relative pose of j as seen from i: \f$ \Delta_i^j = j \ominus i \f$
145  * True if an edge i->j stores the inverse relateive pose, that is, i as seen from j: \f$ \Delta_i^j = i \ominus j \f$
146  */
147  bool edges_store_inverse_poses;
148 
149  /** @} */
150 
151 
152  /** @name I/O file methods
153  @{ */
154 
155  /** 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</a> )
156  * For 2D graphs only VERTEX2 & EDGE2 entries will be saved, and VERTEX3 & EDGE3 entries for 3D graphs.
157  * Note that EQUIV entries will not be saved, but instead several EDGEs will be stored between the same node IDs.
158  * \sa saveToBinaryFile, loadFromTextFile
159  * \exception On any error
160  */
161  inline void saveToTextFile( const std::string &fileName ) const {
162  detail::graph_ops<self_t>::save_graph_of_poses_from_text_file(this,fileName);
163  }
164 
165  /** 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> )
166  * Recognized line entries are: VERTEX2, VERTEX3, EDGE2, EDGE3, EQUIV.
167  * If an unknown entry is found, a warning is dumped to std::cerr (only once for each unknown keyword).
168  * 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).
169  * \param fileName The file to load.
170  * \param collapse_dup_edges If true, \a collapseDuplicatedEdges will be called automatically after loading (note that this operation may take significant time for very large graphs).
171  * \sa loadFromBinaryFile, saveToTextFile
172  * \exception On any error, as a malformed line or loading a 3D graph in a 2D graph.
173  */
174  inline void loadFromTextFile( const std::string &fileName, bool collapse_dup_edges = true ) {
175  detail::graph_ops<self_t>::load_graph_of_poses_from_text_file(this,fileName);
176  if (collapse_dup_edges) this->collapseDuplicatedEdges();
177  }
178 
179  /** @} */
180 
181  /** @name Utility methods
182  @{ */
183 
184  /** Spanning tree computation of a simple estimation of the global coordinates of each node just from the information in all edges, sorted in a Dijkstra tree based on the current "root" node.
185  * Note that "global" coordinates are with respect to the node with the ID specified in \a root.
186  * \note This method takes into account the value of \a edges_store_inverse_poses
187  * \sa node, root
188  */
189  inline void dijkstra_nodes_estimate() { detail::graph_ops<self_t>::graph_of_poses_dijkstra_init(this); }
190 
191  /** Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them.
192  * Upon return, only one edge remains between each pair of nodes with the mean & covariance (or information matrix) corresponding to the Bayesian fusion of all the Gaussians.
193  * \return Overall number of removed edges.
194  */
195  inline size_t collapseDuplicatedEdges() { return detail::graph_ops<self_t>::graph_of_poses_collapse_dup_edges(this); }
196 
197  /** Computes the overall square error from all the pose constraints (edges) with respect to the global poses in \nodes
198  * If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
199  * \sa getEdgeSquareError
200  * \exception std::exception On global poses not in \a nodes
201  */
202  double getGlobalSquareError(bool ignoreCovariances = true) const {
203  double sqErr=0;
204  const typename BASE::edges_map_t::const_iterator last_it=BASE::edges.end();
205  for (typename BASE::edges_map_t::const_iterator itEdge=BASE::edges.begin();itEdge!=last_it;++itEdge)
206  sqErr+=detail::graph_ops<self_t>::graph_edge_sqerror(this,itEdge,ignoreCovariances);
207  return sqErr;
208  }
209 
210  /** Computes the square error of one pose constraints (edge) with respect to the global poses in \nodes
211  * If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
212  * \exception std::exception On global poses not in \a nodes
213  */
214  inline double getEdgeSquareError(const typename BASE::edges_map_t::const_iterator &itEdge, bool ignoreCovariances = true) const { return detail::graph_ops<self_t>::graph_edge_sqerror(this,itEdge,ignoreCovariances); }
215 
216  /** Computes the square error of one pose constraints (edge) with respect to the global poses in \nodes
217  * If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
218  * \exception std::exception On edge not existing or global poses not in \a nodes
219  */
220  double getEdgeSquareError(const TNodeID from_id, const TNodeID to_id, bool ignoreCovariances = true ) const
221  {
222  const typename BASE::edges_map_t::const_iterator itEdge = BASE::edges.find( std::make_pair(from_id,to_id) );
223  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)));
224  return getEdgeSquareError(itEdge,ignoreCovariances);
225  }
226 
227  /** Empty all edges, nodes and set root to ID 0. */
228  inline void clear() {
229  BASE::edges.clear();
230  nodes.clear();
231  root = 0;
232  edges_store_inverse_poses = false;
233  }
234 
235  /** Return number of nodes in the list \nodes of global coordinates (may be differente that all nodes appearing in edges)
236  * \sa mrpt::graphs::CDirectedGraph::countDifferentNodesInEdges
237  */
238  inline size_t nodeCount() const { return nodes.size(); }
239 
240  /** @} */
241 
242  /** @name Ctors & Dtors
243  @{ */
244 
245  /** Default constructor (just sets root to "0" and edges_store_inverse_poses to "false") */
246  inline CNetworkOfPoses() : root(0), edges_store_inverse_poses(false) { }
247  ~CNetworkOfPoses() { }
248  /** @} */
249  };
250 
251 #define DEFINE_SERIALIZABLE_GRAPH \
252  protected: \
253  virtual void writeToStream(CStream &out, int *version) const { \
254  if (version) *version = 0; \
255  else out << nodes << edges << root; \
256  } \
257  virtual void readFromStream(CStream &in, int version) { \
258  switch(version) \
259  { \
260  case 0: { in >> nodes >> edges >> root; } break; \
261  default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version) \
262  }; \
263  }
264 
265 
266 
267  /** \addtogroup mrpt_graphs_grp
268  @{ */
269 
270  // Define serializable versions of the template above for each specific kind of "edge":
271 
272  // This must be added to any CSerializable derived class:
273  DEFINE_SERIALIZABLE_PRE_CUSTOM_LINKAGE( CNetworkOfPoses2D, GRAPHS_IMPEXP )
274  DEFINE_SERIALIZABLE_PRE_CUSTOM_LINKAGE( CNetworkOfPoses3D, GRAPHS_IMPEXP )
275  DEFINE_SERIALIZABLE_PRE_CUSTOM_LINKAGE( CNetworkOfPoses2DCov, GRAPHS_IMPEXP )
276  DEFINE_SERIALIZABLE_PRE_CUSTOM_LINKAGE( CNetworkOfPoses3DCov, GRAPHS_IMPEXP )
277  DEFINE_SERIALIZABLE_PRE_CUSTOM_LINKAGE( CNetworkOfPoses2DInf, GRAPHS_IMPEXP )
278  DEFINE_SERIALIZABLE_PRE_CUSTOM_LINKAGE( CNetworkOfPoses3DInf, GRAPHS_IMPEXP )
279 
280  /** The specialization of CNetworkOfPoses for poses of type CPose2D (not a PDF!), also implementing serialization.
281  * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
282  */
283  class GRAPHS_IMPEXP CNetworkOfPoses2D : public CNetworkOfPoses<CPose2D,map_traits_stdmap>, public mrpt::utils::CSerializable
284  {
285  DEFINE_MRPT_OBJECT( CNetworkOfPoses2D ) // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
286  DEFINE_SERIALIZABLE_GRAPH
287  };
288 
289  /** The specialization of CNetworkOfPoses for poses of type CPose3D (not a PDF!), also implementing serialization.
290  * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
291  */
292  class GRAPHS_IMPEXP CNetworkOfPoses3D : public CNetworkOfPoses<CPose3D,map_traits_stdmap>, public mrpt::utils::CSerializable
293  {
294  DEFINE_MRPT_OBJECT( CNetworkOfPoses3D ) // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
295  DEFINE_SERIALIZABLE_GRAPH
296  };
297 
298  /** The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian, also implementing serialization.
299  * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
300  */
301  class GRAPHS_IMPEXP CNetworkOfPoses2DCov : public CNetworkOfPoses<CPosePDFGaussian,map_traits_stdmap>, public mrpt::utils::CSerializable
302  {
303  DEFINE_MRPT_OBJECT( CNetworkOfPoses2DCov ) // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
304  DEFINE_SERIALIZABLE_GRAPH
305  };
306 
307  /** The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian, also implementing serialization.
308  * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
309  */
310  class GRAPHS_IMPEXP CNetworkOfPoses3DCov : public CNetworkOfPoses<CPose3DPDFGaussian,map_traits_stdmap>, public mrpt::utils::CSerializable
311  {
312  DEFINE_MRPT_OBJECT( CNetworkOfPoses3DCov ) // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
313  DEFINE_SERIALIZABLE_GRAPH
314  };
315 
316  /** The specialization of CNetworkOfPoses for poses of type CPosePDFGaussianInf, also implementing serialization.
317  * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
318  */
319  class GRAPHS_IMPEXP CNetworkOfPoses2DInf : public CNetworkOfPoses<CPosePDFGaussianInf,map_traits_stdmap>, public mrpt::utils::CSerializable
320  {
321  DEFINE_MRPT_OBJECT( CNetworkOfPoses2DInf ) // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
322  DEFINE_SERIALIZABLE_GRAPH
323  };
324 
325  /** The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussianInf, also implementing serialization.
326  * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
327  */
328  class GRAPHS_IMPEXP CNetworkOfPoses3DInf : public CNetworkOfPoses<CPose3DPDFGaussianInf,map_traits_stdmap>, public mrpt::utils::CSerializable
329  {
330  DEFINE_MRPT_OBJECT( CNetworkOfPoses3DInf ) // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
331  DEFINE_SERIALIZABLE_GRAPH
332  };
333 
334  /** @} */ // end of grouping
335  } // End of namespace
336 } // End of namespace
337 
338 
339 // Implementation of templates (in a separate file for clarity)
340 #include "CNetworkOfPoses_impl.h"
341 
342 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013