Main MRPT website > C++ reference
MRPT logo

CNetworkOfPoses.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CONSTRAINED_POSE_NETWORK_H
00029 #define CONSTRAINED_POSE_NETWORK_H
00030 
00031 #include <mrpt/poses/CPosePDFGaussian.h>
00032 #include <mrpt/poses/CPose3DPDFGaussian.h>
00033 #include <mrpt/poses/CPosePDFGaussianInf.h>
00034 #include <mrpt/poses/CPose3DPDFGaussianInf.h>
00035 #include <mrpt/math/graphs.h>
00036 
00037 #include <mrpt/utils/CSerializable.h>
00038 #include <mrpt/utils/CFileGZInputStream.h>
00039 #include <mrpt/utils/CFileGZOutputStream.h>
00040 
00041 #include <mrpt/utils/stl_extensions.h>
00042 
00043 // The main class in this file is CNetworkOfPoses, a generic basic template for
00044 //  predefined 2D/3D graphs of pose contraints.
00045 
00046 namespace mrpt
00047 {
00048         namespace poses
00049         {
00050                 using mrpt::utils::TNodeID;
00051                 template<class CPOSE,class MAPS_IMPLEMENTATION > class CNetworkOfPoses; // Forward decl. needed by detail functions.
00052 
00053                 /** Internal functions for MRPT */
00054                 namespace detail
00055                 {
00056                         template<class CPOSE,class MAPS_IMPLEMENTATION> void BASE_IMPEXP save_graph_of_poses_from_text_file(const CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION> *g, const std::string &fil);
00057                         template<class CPOSE,class MAPS_IMPLEMENTATION> void BASE_IMPEXP load_graph_of_poses_from_text_file(CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION>*g, const std::string &fil);
00058                         template<class CPOSE,class MAPS_IMPLEMENTATION> void BASE_IMPEXP graph_of_poses_dijkstra_init(CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION>*g);
00059                         template<class CPOSE,class MAPS_IMPLEMENTATION> size_t BASE_IMPEXP graph_of_poses_collapse_dup_edges(CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION>*g);
00060                         template<class CPOSE,class MAPS_IMPLEMENTATION> double BASE_IMPEXP graph_edge_sqerror(const CNetworkOfPoses<CPOSE,MAPS_IMPLEMENTATION>*g, const typename mrpt::math::CDirectedGraph<CPOSE>::edges_map_t::const_iterator &itEdge, bool ignoreCovariances );
00061                 }
00062 
00063                 /** A directed graph of pose constraints, with edges being the relative pose between pairs of nodes indentified by their numeric IDs (of type TNodeID).
00064                   *  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".
00065                   *   These poses are stored in the edges in the format specified by the template argument CPOSE. Users should employ the following derived classes
00066                   *   depending on the desired representation of edges:
00067                   *      - mrpt::poses::CNetworkOfPoses2D    : 2D edges as a simple CPose2D (x y phi)
00068                   *      - mrpt::poses::CNetworkOfPoses3D    : 3D edges as a simple CPose3D (x y z yaw pitch roll)
00069                   *      - mrpt::poses::CNetworkOfPoses2DInf : 2D edges as a Gaussian PDF with information matrix ( CPosePDFGaussianInf )
00070                   *      - mrpt::poses::CNetworkOfPoses3DInf : 3D edges as a Gaussian PDF with information matrix ( CPose3DPDFGaussianInf )
00071                   *      - mrpt::poses::CNetworkOfPoses2DCov : 2D edges as a Gaussian PDF with covariance matrix ( CPosePDFGaussian ). It's more efficient to use the information matrix version instead!
00072                   *      - mrpt::poses::CNetworkOfPoses3DCov : 3D edges as a Gaussian PDF with covariance matrix ( CPose3DPDFGaussian ). It's more efficient to use the information matrix version instead!
00073                   *
00074                   *  Two main members store all the information in this class:
00075                   *             - \a edge  (in the base class mrpt::math::CDirectedGraph::edge): A map from pairs of node ID -> pose constraints.
00076                   *             - \a nodes : A map from node ID -> estimated pose of that node (actually, read below on the template argument MAPS_IMPLEMENTATION).
00077                   *
00078                   *  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> ),
00079                   *   using \a loadFromTextFile and \a saveToTextFile.
00080                   *
00081                   *  This class is the base for representing networks of poses, which are the main data type of a series
00082                   *   of SLAM algorithms implemented in the library mrpt-slam, in the namespace mrpt::graphslam.
00083                   *
00084                   *  For tools to visualize graphs as 2D/3D plots, see the namespace mrpt::opengl::graph_tools in the library mrpt-opengl.
00085                   *
00086                   *  The template arguments are:
00087                   *             - CPOSE: The type of the edges, which hold a relative pose (2D/3D, just a value or a Gaussian, etc.)
00088                   *             - 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).
00089                   *
00090                   * \sa mrpt::graphslam
00091                   */
00092                 template<class CPOSE, class MAPS_IMPLEMENTATION = map_traits_stdmap >
00093                 class CNetworkOfPoses : public mrpt::math::CDirectedGraph< CPOSE >
00094                 {
00095                 public:
00096                         /** @name Typedef's
00097                             @{ */
00098 
00099                         /** The base class "CDirectedGraph<CPOSE>" */
00100                         typedef mrpt::math::CDirectedGraph< CPOSE > BASE;
00101 
00102                         /** The type of PDF poses in the contraints (edges) (=CPOSE template argument) */
00103                         typedef CPOSE                               constraint_t;
00104 
00105                         /** The type of map's implementation (=MAPS_IMPLEMENTATION template argument) */
00106                         typedef MAPS_IMPLEMENTATION                 maps_implementation_t;
00107 
00108                         /** A map from pose IDs to their global coordinates estimates, with uncertainty */
00109                         typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,CPOSE>     global_poses_pdf_t;
00110 
00111                         /** A map from pose IDs to their global coordinates estimates, without uncertainty (the "most-likely value") */
00112                         typedef typename MAPS_IMPLEMENTATION::template map<TNodeID,typename CPOSE::type_value> global_poses_t;
00113 
00114                         /** @} */
00115 
00116 
00117                         /** @name Data members
00118                             @{ */
00119 
00120                         /** The nodes (vertices) of the graph, with their estimated "global" (with respect to \a root) position, without an associated covariance.
00121                           * \sa dijkstra_nodes_estimate
00122                           */
00123                         global_poses_t  nodes;
00124 
00125                         /** 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". */
00126                         TNodeID         root;
00127 
00128                         /** @} */
00129 
00130 
00131                         /** @name I/O file methods
00132                             @{ */
00133 
00134                         /** 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> )
00135                           *  For 2D graphs only VERTEX2 & EDGE2 entries will be saved, and VERTEX3 & EDGE3 entries for 3D graphs.
00136                           *  Note that EQUIV entries will not be saved, but instead several EDGEs will be stored between the same node IDs.
00137                           * \sa saveToBinaryFile, loadFromTextFile
00138                           * \exception On any error
00139                           */
00140                         inline void saveToTextFile( const std::string &fileName ) const {
00141                                 mrpt::poses::detail::save_graph_of_poses_from_text_file(this,fileName);
00142                         }
00143 
00144                         /** 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> )
00145                           *   Recognized line entries are: VERTEX2, VERTEX3, EDGE2, EDGE3, EQUIV.
00146                           *   If an unknown entry is found, a warning is dumped to std::cerr (only once for each unknown keyword).
00147                           *   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).
00148                           * \param fileName The file to load.
00149                           * \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).
00150                           * \sa loadFromBinaryFile, saveToTextFile
00151                           * \exception On any error, as a malformed line or loading a 3D graph in a 2D graph.
00152                           */
00153                         inline void loadFromTextFile( const std::string &fileName, bool collapse_dup_edges = true ) {
00154                                 mrpt::poses::detail::load_graph_of_poses_from_text_file(this,fileName);
00155                                 if (collapse_dup_edges) this->collapseDuplicatedEdges();
00156                         }
00157 
00158                         /** @} */
00159 
00160                         /** @name Utility methods
00161                             @{ */
00162 
00163                         /** 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.
00164                           *  Note that "global" coordinates are with respect to the node with the ID specified in \a root.
00165                           * \sa node, root
00166                           */
00167                         inline void dijkstra_nodes_estimate() { detail::graph_of_poses_dijkstra_init(this); }
00168 
00169                         /** Look for duplicated edges (even in opposite directions) between all pairs of nodes and fuse them.
00170                           *  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.
00171                           * \return Overall number of removed edges.
00172                           */
00173                         inline size_t collapseDuplicatedEdges() { return detail::graph_of_poses_collapse_dup_edges(this); }
00174 
00175                         /** Computes the overall square error from all the pose constraints (edges) with respect to the global poses in \nodes
00176                           *  If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
00177                           * \sa getEdgeSquareError
00178                           * \exception std::exception On global poses not in \a nodes
00179                           */
00180                         double getGlobalSquareError(bool ignoreCovariances = true) const {
00181                                 double sqErr=0;
00182                                 const typename BASE::edges_map_t::const_iterator last_it=BASE::edges.end();
00183                                 for (typename BASE::edges_map_t::const_iterator itEdge=BASE::edges.begin();itEdge!=last_it;++itEdge)
00184                                         sqErr+=detail::graph_edge_sqerror(this,itEdge,ignoreCovariances);
00185                                 return sqErr;
00186                         }
00187 
00188                         /** Computes the square error of one pose constraints (edge) with respect to the global poses in \nodes
00189                           *  If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
00190                           * \exception std::exception On global poses not in \a nodes
00191                           */
00192                         inline double getEdgeSquareError(const typename BASE::edges_map_t::const_iterator &itEdge, bool ignoreCovariances = true) const { return detail::graph_edge_sqerror(this,itEdge,ignoreCovariances); }
00193 
00194                         /** Computes the square error of one pose constraints (edge) with respect to the global poses in \nodes
00195                           *  If \a ignoreCovariances is false, the squared Mahalanobis distance will be computed instead of the straight square error.
00196                           * \exception std::exception On edge not existing or global poses not in \a nodes
00197                           */
00198                         double getEdgeSquareError(const TNodeID from_id, const TNodeID to_id, bool ignoreCovariances = true ) const
00199                         {
00200                                 const typename BASE::edges_map_t::const_iterator itEdge = BASE::edges.find( std::make_pair(from_id,to_id) );
00201                                 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)));
00202                                 return getEdgeSquareError(itEdge,ignoreCovariances);
00203                         }
00204 
00205                         /** Empty all edges, nodes and set root to ID 0. */
00206                         inline void clear() {
00207                                 BASE::edges.clear();
00208                                 nodes.clear();
00209                                 root = 0;
00210                         }
00211 
00212                         /** Return number of nodes in the list \nodes of global coordinates (may be differente that all nodes appearing in edges)
00213                           * \sa mrpt::math::CDirectedGraph::countDifferentNodesInEdges
00214                           */
00215                         inline size_t nodeCount() const { return nodes.size(); }
00216 
00217                         /**  @} */
00218 
00219                         /** @name Ctors & Dtors
00220                             @{ */
00221 
00222                         /** Default constructor (just sets root to "0") */
00223                         inline CNetworkOfPoses() : root(0) { }
00224                         ~CNetworkOfPoses() { }
00225                         /** @} */
00226                 };
00227 
00228 #define DEFINE_SERIALIZABLE_GRAPH  \
00229                 protected: \
00230                         virtual void  writeToStream(CStream &out, int *version) const { \
00231                                 if (version) *version = 0; \
00232                                 else out << nodes << edges << root;  \
00233                         } \
00234                         virtual void readFromStream(CStream &in, int version) { \
00235                                 switch(version) \
00236                                 { \
00237                                 case 0: { in >> nodes >> edges >> root; } break; \
00238                                 default: MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version) \
00239                                 }; \
00240                         }
00241 
00242 
00243 
00244                 // Define serializable versions of the template above for each specific kind of "edge":
00245 
00246                 // This must be added to any CSerializable derived class:
00247                 DEFINE_SERIALIZABLE_PRE( CNetworkOfPoses2D )
00248                 DEFINE_SERIALIZABLE_PRE( CNetworkOfPoses3D )
00249                 DEFINE_SERIALIZABLE_PRE( CNetworkOfPoses2DCov )
00250                 DEFINE_SERIALIZABLE_PRE( CNetworkOfPoses3DCov )
00251                 DEFINE_SERIALIZABLE_PRE( CNetworkOfPoses2DInf )
00252                 DEFINE_SERIALIZABLE_PRE( CNetworkOfPoses3DInf )
00253 
00254                 /** The specialization of CNetworkOfPoses for poses of type CPose2D (not a PDF!), also implementing serialization.
00255                   * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
00256                   */
00257                 class BASE_IMPEXP CNetworkOfPoses2D : public CNetworkOfPoses<CPose2D,map_traits_stdmap>, public mrpt::utils::CSerializable
00258                 {
00259                         DEFINE_MRPT_OBJECT( CNetworkOfPoses2D ) // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
00260                         DEFINE_SERIALIZABLE_GRAPH
00261                 };
00262 
00263                 /** The specialization of CNetworkOfPoses for poses of type CPose3D (not a PDF!), also implementing serialization.
00264                   * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
00265                   */
00266                 class BASE_IMPEXP CNetworkOfPoses3D : public CNetworkOfPoses<CPose3D,map_traits_stdmap>, public mrpt::utils::CSerializable
00267                 {
00268                         DEFINE_MRPT_OBJECT( CNetworkOfPoses3D ) // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
00269                         DEFINE_SERIALIZABLE_GRAPH
00270                 };
00271 
00272                 /** The specialization of CNetworkOfPoses for poses of type CPosePDFGaussian, also implementing serialization.
00273                   * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
00274                   */
00275                 class BASE_IMPEXP CNetworkOfPoses2DCov : public CNetworkOfPoses<CPosePDFGaussian,map_traits_stdmap>, public mrpt::utils::CSerializable
00276                 {
00277                         DEFINE_MRPT_OBJECT( CNetworkOfPoses2DCov )      // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
00278                         DEFINE_SERIALIZABLE_GRAPH
00279                 };
00280 
00281                 /** The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussian, also implementing serialization.
00282                   * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
00283                   */
00284                 class BASE_IMPEXP CNetworkOfPoses3DCov : public CNetworkOfPoses<CPose3DPDFGaussian,map_traits_stdmap>, public mrpt::utils::CSerializable
00285                 {
00286                         DEFINE_MRPT_OBJECT( CNetworkOfPoses3DCov )      // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
00287                         DEFINE_SERIALIZABLE_GRAPH
00288                 };
00289 
00290                 /** The specialization of CNetworkOfPoses for poses of type CPosePDFGaussianInf, also implementing serialization.
00291                   * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
00292                   */
00293                 class BASE_IMPEXP CNetworkOfPoses2DInf : public CNetworkOfPoses<CPosePDFGaussianInf,map_traits_stdmap>, public mrpt::utils::CSerializable
00294                 {
00295                         DEFINE_MRPT_OBJECT( CNetworkOfPoses2DInf )      // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
00296                         DEFINE_SERIALIZABLE_GRAPH
00297                 };
00298 
00299                 /** The specialization of CNetworkOfPoses for poses of type CPose3DPDFGaussianInf, also implementing serialization.
00300                   * \sa CNetworkOfPoses, CNetworkOfPoses2D, CNetworkOfPoses3D, CNetworkOfPoses2DInf, CNetworkOfPoses3DInf
00301                   */
00302                 class BASE_IMPEXP CNetworkOfPoses3DInf : public CNetworkOfPoses<CPose3DPDFGaussianInf,map_traits_stdmap>, public mrpt::utils::CSerializable
00303                 {
00304                         DEFINE_MRPT_OBJECT( CNetworkOfPoses3DInf )      // Should be DEFINE_SERIALIZABLE but the next macro defines what that macro only declared.
00305                         DEFINE_SERIALIZABLE_GRAPH
00306                 };
00307 
00308         } // End of namespace
00309 } // End of namespace
00310 
00311 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011