Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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 
18 
22 
23 #include <mrpt/utils/traits_map.h>
25 #include <mrpt/poses/poses_frwds.h>
26 
27 namespace mrpt
28 {
29  namespace graphs
30  {
31  /** Internal functions for MRPT */
32  namespace detail
33  {
34  template <class GRAPH_T> struct graph_ops;
35  /** An empty structure */
37  }
38 
39  /** A directed graph of pose constraints, with edges being the relative pose between pairs of nodes indentified by their numeric IDs (of type mrpt::utils::TNodeID).
40  * 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".
41  * These poses are stored in the edges in the format specified by the template argument CPOSE. Users should employ the following derived classes
42  * depending on the desired representation of edges:
43  * - mrpt::graphs::CNetworkOfPoses2D : 2D edges as a simple CPose2D (x y phi)
44  * - mrpt::graphs::CNetworkOfPoses3D : 3D edges as a simple mrpt::poses::CPose3D (x y z yaw pitch roll)
45  * - mrpt::graphs::CNetworkOfPoses2DInf : 2D edges as a Gaussian PDF with information matrix ( CPosePDFGaussianInf )
46  * - mrpt::graphs::CNetworkOfPoses3DInf : 3D edges as a Gaussian PDF with information matrix ( CPose3DPDFGaussianInf )
47  * - mrpt::graphs::CNetworkOfPoses2DCov : 2D edges as a Gaussian PDF with covariance matrix ( CPosePDFGaussian ). It's more efficient to use the information matrix version instead!
48  * - mrpt::graphs::CNetworkOfPoses3DCov : 3D edges as a Gaussian PDF with covariance matrix ( CPose3DPDFGaussian ). It's more efficient to use the information matrix version instead!
49  *
50  * Two main members store all the information in this class:
51  * - \a edges (in the base class mrpt::graphs::CDirectedGraph::edges): A map from pairs of node ID -> pose constraints.
52  * - \a nodes : A map from node ID -> estimated pose of that node (actually, read below on the template argument MAPS_IMPLEMENTATION).
53  *
54  * 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> ),
55  * using \a loadFromTextFile and \a saveToTextFile.
56  *
57  * This class is the base for representing networks of poses, which are the main data type of a series
58  * of SLAM algorithms implemented in the library mrpt-slam, in the namespace mrpt::graphslam.
59  *
60  * For tools to visualize graphs as 2D/3D plots, see the namespace mrpt::opengl::graph_tools in the library mrpt-opengl.
61  *
62  * The template arguments are:
63  * - CPOSE: The type of the edges, which hold a relative pose (2D/3D, just a value or a Gaussian, etc.)
64  * - 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).
65  *
66  * \sa mrpt::graphslam
67  * \ingroup mrpt_graphs_grp
68  */
69  template<
70  class CPOSE, // Type of edges
71  class MAPS_IMPLEMENTATION = mrpt::utils::map_traits_stdmap, // Use std::map<> vs. std::vector<>
72  class NODE_ANNOTATIONS = mrpt::graphs::detail::node_annotations_empty,
73  class EDGE_ANNOTATIONS = mrpt::graphs::detail::edge_annotations_empty
74  >
75  class CNetworkOfPoses : public mrpt::graphs::CDirectedGraph< CPOSE, EDGE_ANNOTATIONS >
76  {
77  public:
78  /** @name Typedef's
79  @{ */
80  typedef mrpt::graphs::CDirectedGraph<CPOSE,EDGE_ANNOTATIONS> BASE; //!< The base class "CDirectedGraph<CPOSE,EDGE_ANNOTATIONS>" */
82 
83  typedef CPOSE constraint_t; //!< The type of PDF poses in the contraints (edges) (=CPOSE template argument)
84  typedef NODE_ANNOTATIONS node_annotations_t; //!< The extra annotations in nodes, apart from a \a constraint_no_pdf_t
85  typedef EDGE_ANNOTATIONS edge_annotations_t; //!< The extra annotations in edges, apart from a \a constraint_t
86 
87  typedef MAPS_IMPLEMENTATION maps_implementation_t; //!< The type of map's implementation (=MAPS_IMPLEMENTATION template argument)
88  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)
89 
90  /** 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 */
91  struct global_pose_t : public constraint_no_pdf_t, public NODE_ANNOTATIONS
92  {
93  // Replicate possible constructors:
94  inline global_pose_t() : constraint_no_pdf_t() { }
95  template <typename ARG1> inline global_pose_t(const ARG1 &a1) : constraint_no_pdf_t(a1) { }
96  template <typename ARG1,typename ARG2> inline global_pose_t(const ARG1 &a1,const ARG2 &a2) : constraint_no_pdf_t(a1,a2) { }
97  };
98 
99  /** A map from pose IDs to their global coordinates estimates, with uncertainty */
100  typedef typename MAPS_IMPLEMENTATION::template map<mrpt::utils::TNodeID,CPOSE> global_poses_pdf_t;
101 
102  /** A map from pose IDs to their global coordinates estimates, without uncertainty (the "most-likely value") */
103  typedef typename MAPS_IMPLEMENTATION::template map<mrpt::utils::TNodeID,global_pose_t> global_poses_t;
104 
105  /** @} */
106 
107 
108  /** @name Data members
109  @{ */
110 
111  /** The nodes (vertices) of the graph, with their estimated "global" (with respect to \a root) position, without an associated covariance.
112  * \sa dijkstra_nodes_estimate
113  */
114  global_poses_t nodes;
115 
116  /** 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". */
118 
119  /** 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$
120  * 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$
121  */
123 
124  /** @} */
125 
126 
127  /** @name I/O file methods
128  @{ */
129 
130  /** 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> )
131  * For 2D graphs only VERTEX2 & EDGE2 entries will be saved, and VERTEX3 & EDGE3 entries for 3D graphs.
132  * Note that EQUIV entries will not be saved, but instead several EDGEs will be stored between the same node IDs.
133  * \sa saveToBinaryFile, loadFromTextFile
134  * \exception On any error
135  */
136  inline void saveToTextFile( const std::string &fileName ) const {
138  }
139 
140  /** 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> )
141  * Recognized line entries are: VERTEX2, VERTEX3, EDGE2, EDGE3, EQUIV.
142  * If an unknown entry is found, a warning is dumped to std::cerr (only once for each unknown keyword).
143  * 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).
144  * \param fileName The file to load.
145  * \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).
146  * \sa loadFromBinaryFile, saveToTextFile
147  * \exception On any error, as a malformed line or loading a 3D graph in a 2D graph.
148  */
149  inline void loadFromTextFile( const std::string &fileName, bool collapse_dup_edges = true ) {
151  if (collapse_dup_edges) this->collapseDuplicatedEdges();
152  }
153 
154  /** @} */
155 
156  /** @name Utility methods
157  @{ */
158 
159  /** 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.
160  * Note that "global" coordinates are with respect to the node with the ID specified in \a root.
161  * \note This method takes into account the value of \a edges_store_inverse_poses
162  * \sa node, root
163  */
165 
166  /** Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them.
167  * 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.
168  * \return Overall number of removed edges.
169  */
171 
172  /** Computes the overall square error from all the pose constraints (edges) with respect to the global poses in \nodes
173  * If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
174  * \sa getEdgeSquareError
175  * \exception std::exception On global poses not in \a nodes
176  */
177  double getGlobalSquareError(bool ignoreCovariances = true) const {
178  double sqErr=0;
179  const typename BASE::edges_map_t::const_iterator last_it=BASE::edges.end();
180  for (typename BASE::edges_map_t::const_iterator itEdge=BASE::edges.begin();itEdge!=last_it;++itEdge)
181  sqErr+=detail::graph_ops<self_t>::graph_edge_sqerror(this,itEdge,ignoreCovariances);
182  return sqErr;
183  }
184 
185  /** Computes the square error of one pose constraints (edge) with respect to the global poses in \nodes
186  * If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
187  * \exception std::exception On global poses not in \a nodes
188  */
189  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); }
190 
191  /** Computes the square error of one pose constraints (edge) with respect to the global poses in \nodes
192  * If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
193  * \exception std::exception On edge not existing or global poses not in \a nodes
194  */
195  double getEdgeSquareError(const mrpt::utils::TNodeID from_id, const mrpt::utils::TNodeID to_id, bool ignoreCovariances = true ) const
196  {
197  const typename BASE::edges_map_t::const_iterator itEdge = BASE::edges.find( std::make_pair(from_id,to_id) );
198  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)));
199  return getEdgeSquareError(itEdge,ignoreCovariances);
200  }
201 
202  /** Empty all edges, nodes and set root to ID 0. */
203  inline void clear() {
204  BASE::edges.clear();
205  nodes.clear();
206  root = 0;
207  edges_store_inverse_poses = false;
208  }
209 
210  /** Return number of nodes in the list \nodes of global coordinates (may be differente that all nodes appearing in edges)
211  * \sa mrpt::graphs::CDirectedGraph::countDifferentNodesInEdges
212  */
213  inline size_t nodeCount() const { return nodes.size(); }
214 
215  /** @} */
216 
217  /** @name Ctors & Dtors
218  @{ */
219 
220  /** Default constructor (just sets root to "0" and edges_store_inverse_poses to "false") */
221  inline CNetworkOfPoses() : root(0), edges_store_inverse_poses(false) { }
223  /** @} */
224  };
225 
226 
227  /** Binary serialization (write) operator "stream << graph" */
228  template <class CPOSE,class MAPS_IMPLEMENTATION,class NODE_ANNOTATIONS,class EDGE_ANNOTATIONS>
229  mrpt::utils::CStream & operator << (mrpt::utils::CStream&out, const CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION,NODE_ANNOTATIONS,EDGE_ANNOTATIONS> &obj)
230  {
233  return out;
234  }
235 
236  /** Binary serialization (read) operator "stream >> graph" */
237  template <class CPOSE,class MAPS_IMPLEMENTATION,class NODE_ANNOTATIONS,class EDGE_ANNOTATIONS>
239  {
242  return in;
243  }
244 
245  /** \addtogroup mrpt_graphs_grp
246  @{ */
247 
248  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.
249  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.
250  typedef CNetworkOfPoses<mrpt::poses::CPosePDFGaussian,mrpt::utils::map_traits_stdmap> CNetworkOfPoses2DCov; //!< The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian, also implementing serialization.
251  typedef CNetworkOfPoses<mrpt::poses::CPose3DPDFGaussian,mrpt::utils::map_traits_stdmap> CNetworkOfPoses3DCov; //!< The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian, also implementing serialization.
252  typedef CNetworkOfPoses<mrpt::poses::CPosePDFGaussianInf,mrpt::utils::map_traits_stdmap> CNetworkOfPoses2DInf; //!< The specialization of CNetworkOfPoses for poses of type CPosePDFGaussianInf, also implementing serialization.
253  typedef CNetworkOfPoses<mrpt::poses::CPose3DPDFGaussianInf,mrpt::utils::map_traits_stdmap> CNetworkOfPoses3DInf; //!< The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussianInf, also implementing serialization.
254 
255  /** @} */ // end of grouping
256 
257  } // End of namespace
258 
259  // Specialization of TTypeName must occur in the same namespace:
260  namespace utils
261  {
262  // Extensions to mrpt::utils::TTypeName for matrices:
263  template<
264  class CPOSE,
265  class MAPS_IMPLEMENTATION,
266  class NODE_ANNOTATIONS,
267  class EDGE_ANNOTATIONS
268  >
269  struct TTypeName <mrpt::graphs::CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION,NODE_ANNOTATIONS,EDGE_ANNOTATIONS> >
270  {
271  static std::string get()
272  {
273  return std::string("mrpt::graphs::CNetworkOfPoses<")
274  +TTypeName<CPOSE>::get() + std::string(",")
275  +TTypeName<MAPS_IMPLEMENTATION>::get() + std::string(",")
276  +TTypeName<NODE_ANNOTATIONS>::get() + std::string(",")
278  +std::string(">");
279  }
280  };
281 
282 
286 
287  }
288 
289 } // End of namespace
290 
291 
292 // Implementation of templates (in a separate file for clarity)
293 #include "CNetworkOfPoses_impl.h"
294 
295 #endif
A directed graph with the argument of the template specifying the type of the annotations in the edge...
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, global_pose_t > global_poses_t
A map from pose IDs to their global coordinates estimates, without uncertainty (the "most-likely valu...
void dijkstra_nodes_estimate()
Spanning tree computation of a simple estimation of the global coordinates of each node just from the...
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
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.
CNetworkOfPoses< mrpt::poses::CPose3DPDFGaussianInf, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3DInf
The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussianInf, also implementing seri...
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
size_t nodeCount() const
Return number of nodes in the list of global coordinates (may be differente that all nodes appearing...
size_t collapseDuplicatedEdges()
Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
void 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 lin...
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
void saveToTextFile(const std::string &fileName) const
Saves to a text file in the format used by TORO & HoG-man (more on the format here ) For 2D graphs on...
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
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 If igno...
static void graph_of_poses_dijkstra_init(graph_t *g)
A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...
CNetworkOfPoses< mrpt::poses::CPosePDFGaussianInf, mrpt::utils::map_traits_stdmap > CNetworkOfPoses2DInf
The specialization of CNetworkOfPoses for poses of type CPosePDFGaussianInf, also implementing serial...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
uint64_t TNodeID
The type for node IDs in graphs of different types.
CNetworkOfPoses()
Default constructor (just sets root to "0" and edges_store_inverse_poses to "false") ...
The type of each global pose in nodes: an extension of the constraint_no_pdf_t pose with any optional...
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)
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.
CPOSE constraint_t
The type of PDF poses in the contraints (edges) (=CPOSE template argument)
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, CPOSE > global_poses_pdf_t
A map from pose IDs to their global coordinates estimates, with uncertainty.
static double graph_edge_sqerror(const graph_t *g, const typename mrpt::graphs::CDirectedGraph< typename graph_t::constraint_t >::edges_map_t::const_iterator &itEdge, bool ignoreCovariances)
a helper struct with static template functions
mrpt::graphs::CDirectedGraph< CPOSE, EDGE_ANNOTATIONS > BASE
The base class "CDirectedGraph<CPOSE,EDGE_ANNOTATIONS>" */.
mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > &obj)
Binary serialization (read) operator "stream >> graph".
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
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 If igno...
CNetworkOfPoses< mrpt::poses::CPose3DPDFGaussian, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3DCov
The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian, also implementing seriali...
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)
mrpt::utils::TNodeID root
The ID of the node that is the origin of coordinates, used as reference by all coordinates in ...
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...
EDGE_ANNOTATIONS edge_annotations_t
The extra annotations in edges, apart from a constraint_t.
CNetworkOfPoses< mrpt::poses::CPose3D, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3D
The specialization of CNetworkOfPoses for poses of type mrpt::poses::CPose3D (not a PDF!)...
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::CPosePDFGaussian, mrpt::utils::map_traits_stdmap > CNetworkOfPoses2DCov
The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian, also implementing serializa...
MAPS_IMPLEMENTATION maps_implementation_t
The type of map&#39;s implementation (=MAPS_IMPLEMENTATION template argument)
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...
#define MRPT_DECLARE_TTYPENAME(_TYPE)
Definition: TTypeName.h:60



Page generated by Doxygen 1.8.11 for MRPT 1.4.0 SVN:Unversioned directory at Mon Jul 4 10:31:07 UTC 2016