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.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:46:17 UTC 2011 |