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
26
27namespace 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,
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:
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 */
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;
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") */
223 /** @} */
224 };
225
226
227 /** Binary serialization (write) operator "stream << graph" */
228 template <class CPOSE,class MAPS_IMPLEMENTATION,class NODE_ANNOTATIONS,class EDGE_ANNOTATIONS>
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
#define MRPT_DECLARE_TTYPENAME(_TYPE)
Definition: TTypeName.h:60
A directed graph with the argument of the template specifying the type of the annotations in the edge...
edges_map_t edges
The public member with the directed edges in the graph.
edges_map_t::const_iterator const_iterator
A directed graph of pose constraints, with edges being the relative pose between pairs of nodes inden...
MAPS_IMPLEMENTATION::template map< mrpt::utils::TNodeID, CPOSE > global_poses_pdf_t
A map from pose IDs to their global coordinates estimates, with uncertainty.
CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > self_t
My own type.
void clear()
Empty all edges, nodes and set root to ID 0.
MAPS_IMPLEMENTATION maps_implementation_t
The type of map's implementation (=MAPS_IMPLEMENTATION template argument)
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 I...
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...
global_poses_t nodes
The nodes (vertices) of the graph, with their estimated "global" (with respect to root) position,...
size_t collapseDuplicatedEdges()
Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them.
EDGE_ANNOTATIONS edge_annotations_t
The extra annotations in edges, apart from a constraint_t.
void dijkstra_nodes_estimate()
Spanning tree computation of a simple estimation of the global coordinates of each node just from the...
mrpt::utils::TNodeID root
The ID of the node that is the origin of coordinates, used as reference by all coordinates in \nodes.
CPOSE::type_value constraint_no_pdf_t
The type of edges or their means if they are PDFs (that is, a simple "edge" value)
CPOSE constraint_t
The type of PDF poses in the contraints (edges) (=CPOSE template argument)
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...
NODE_ANNOTATIONS node_annotations_t
The extra annotations in nodes, apart from a constraint_no_pdf_t.
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...
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...
mrpt::graphs::CDirectedGraph< CPOSE, EDGE_ANNOTATIONS > BASE
The base class "CDirectedGraph<CPOSE,EDGE_ANNOTATIONS>" *‍/.
size_t nodeCount() const
Return number of nodes in the list \nodes of global coordinates (may be differente that all nodes app...
double getGlobalSquareError(bool ignoreCovariances=true) const
Computes the overall square error from all the pose constraints (edges) with respect to the global po...
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 I...
CNetworkOfPoses()
Default constructor (just sets root to "0" and edges_store_inverse_poses to "false")
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
CNetworkOfPoses< mrpt::poses::CPose3DPDFGaussianInf, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3DInf
The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussianInf, also implementing seri...
CNetworkOfPoses< mrpt::poses::CPose3DPDFGaussian, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3DCov
The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian, also implementing seriali...
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...
CNetworkOfPoses< mrpt::poses::CPose3D, mrpt::utils::map_traits_stdmap > CNetworkOfPoses3D
The specialization of CNetworkOfPoses for poses of type mrpt::poses::CPose3D (not a PDF!...
CNetworkOfPoses< mrpt::poses::CPosePDFGaussianInf, mrpt::utils::map_traits_stdmap > CNetworkOfPoses2DInf
The specialization of CNetworkOfPoses for poses of type CPosePDFGaussianInf, also implementing serial...
CNetworkOfPoses< mrpt::poses::CPosePDFGaussian, mrpt::utils::map_traits_stdmap > CNetworkOfPoses2DCov
The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian, also implementing serializa...
uint64_t TNodeID
The type for node IDs in graphs of different types.
Definition: types_simple.h:45
#define ASSERTMSG_(f, __ERROR_MSG)
Definition: mrpt_macros.h:260
mrpt::utils::CStream & operator>>(mrpt::utils::CStream &in, CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > &obj)
Binary serialization (read) operator "stream >> graph".
mrpt::utils::CStream & operator<<(mrpt::utils::CStream &out, const CNetworkOfPoses< CPOSE, MAPS_IMPLEMENTATION, NODE_ANNOTATIONS, EDGE_ANNOTATIONS > &obj)
Binary serialization (write) operator "stream << graph".
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
The type of each global pose in nodes: an extension of the constraint_no_pdf_t pose with any optional...
global_pose_t(const ARG1 &a1, const ARG2 &a2)
a helper struct with static template functions
static void graph_of_poses_dijkstra_init(graph_t *g)
static void read_graph_of_poses_from_binary_file(graph_t *g, mrpt::utils::CStream &in)
static size_t graph_of_poses_collapse_dup_edges(graph_t *g)
static void load_graph_of_poses_from_text_file(graph_t *g, const std::string &fil)
static void save_graph_of_poses_to_binary_file(const graph_t *g, mrpt::utils::CStream &out)
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)
static void save_graph_of_poses_to_text_file(const graph_t *g, const std::string &fil)
A template to obtain the type of its argument as a string at compile time.
Definition: TTypeName.h:48
static std::string get()
Definition: TTypeName.h:49
Traits for using a mrpt::utils::map_as_vector<> (dense, fastest representation)
Definition: traits_map.h:32
Traits for using a std::map<> (sparse representation)
Definition: traits_map.h:25



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Tue Dec 27 00:53:09 UTC 2022