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 CHierarchicalMapMHPartition_H 00029 #define CHierarchicalMapMHPartition_H 00030 00031 #include <mrpt/hmtslam/CLocalMetricHypothesis.h> 00032 #include <mrpt/hmtslam/CHMHMapArc.h> 00033 #include <mrpt/hmtslam/CHMHMapNode.h> 00034 00035 #include <mrpt/poses/CPose3DPDFSOG.h> 00036 #include <mrpt/poses/CPose3DPDFGaussian.h> 00037 #include <mrpt/utils/CDebugOutputCapable.h> 00038 00039 #include <map> 00040 00041 namespace mrpt 00042 { 00043 namespace opengl 00044 { 00045 class COpenGLScene; 00046 } 00047 namespace poses 00048 { 00049 class CPose3DPDFParticles; 00050 } 00051 00052 namespace hmtslam 00053 { 00054 using namespace mrpt::opengl; 00055 using namespace mrpt::slam; 00056 using namespace mrpt::utils; 00057 00058 /** Represents a set of nodes and arcs, posibly only a part of the whole hierarchical, multi-hypothesis map. 00059 * A usar will never create an instance of this class, rather it will employ CHierarchicalMHMap. 00060 * \sa CHierarchicalMHMap, CHMHMapArc, CHMHMapNode 00061 */ 00062 class HMTSLAM_IMPEXP CHierarchicalMapMHPartition : public mrpt::utils::CDebugOutputCapable 00063 { 00064 protected: 00065 /** The internal list of nodes and arcs in the whole hierarchical model. 00066 * The objects must be deleted only in the CHierarchicalMap class, not in partitions only objects. 00067 */ 00068 TNodeList m_nodes; 00069 TArcList m_arcs; 00070 00071 public: 00072 00073 typedef TNodeList::iterator iterator; 00074 typedef TNodeList::const_iterator const_iterator; 00075 00076 /** Returns an iterator to the first node in the graph. */ 00077 const_iterator begin() const { return m_nodes.begin(); } 00078 00079 /** Returns an iterator to the first node in the graph. */ 00080 iterator begin() { return m_nodes.begin(); } 00081 00082 /** Returns an iterator to the end of the list of nodes in the graph. */ 00083 const_iterator end() const { return m_nodes.end(); } 00084 00085 /** Returns an iterator to the end of the list of nodes in the graph. */ 00086 iterator end() { return m_nodes.end(); } 00087 00088 00089 CHierarchicalMapMHPartition() : m_nodes(), m_arcs() 00090 { } 00091 00092 /** A type that reprensents a sequence of node IDs 00093 */ 00094 typedef std::vector<CHMHMapNode::TNodeID> TNodeIDsList; 00095 00096 /** Returns the number of nodes in the partition: 00097 */ 00098 size_t nodeCount() const; 00099 00100 /** Returns the number of arcs in the partition: 00101 */ 00102 size_t arcCount() const; 00103 00104 /** Returns the first node in the graph, or NULL if it does not exist. 00105 * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate" 00106 */ 00107 CHMHMapNodePtr getFirstNode(); 00108 00109 /** Returns the node with the given ID, or NULL if it does not exist. 00110 * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate" 00111 */ 00112 CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id); 00113 00114 /** Returns the node with the given ID, or NULL if it does not exist. 00115 * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate" 00116 */ 00117 const CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id) const; 00118 00119 /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist. 00120 * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate" 00121 */ 00122 CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID ); 00123 00124 /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist. 00125 * \return A pointer to the object. DO NOT DELETE this object, if you want to modify it in someway, first obtain a copy by invoking "CSerializable::duplicate" 00126 */ 00127 const CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID) const; 00128 00129 /** Returns a partition of this graph only with nodes at a given level in the hierarchy (0=ground level,1=parent level,etc) 00130 * - The partition may be empty if no node fulfills the condition. 00131 * - All arcs STARTING at each node from the partition will be added to the partition as well. 00132 * - Levels in the hierarchy here stands for arcs of type "arcType_Belongs" only. 00133 * \sa CHMHMapArc 00134 */ 00135 //CHierarchicalMapMHPartition getPartitionByHiearchyLevel( unsigned int level ); 00136 00137 /** Saves a MATLAB script that represents graphically the nodes with <i>type</i>="Area" in this hierarchical-map(partition), using the stated node as global coordinates reference. 00138 * ADDITIONAL NOTES: 00139 * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node. 00140 * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised. 00141 */ 00142 void saveAreasDiagramForMATLAB( 00143 const std::string &filName, 00144 const CHMHMapNode::TNodeID &idReferenceNode, 00145 const THypothesisID &hypothesisID) const; 00146 00147 /** Saves a MATLAB script that represents graphically the nodes with <i>type</i>="Area" in this hierarchical-map(partition), using the stated node as global coordinates reference, and drawing the ellipses of the localization uncertainty for each node. 00148 * ADDITIONAL NOTES: 00149 * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node. 00150 * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised. 00151 */ 00152 void saveAreasDiagramWithEllipsedForMATLAB( 00153 const std::string &filName, 00154 const CHMHMapNode::TNodeID &idReferenceNode, 00155 const THypothesisID &hypothesisID, 00156 float uncertaintyExagerationFactor = 1.0f, 00157 bool drawArcs = false, 00158 unsigned int numberOfIterationsForOptimalGlobalPoses = 4 00159 ) const; 00160 00161 /** Saves a MATLAB script that represents graphically the reconstructed "global map" 00162 * ADDITIONAL NOTES: 00163 * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node. 00164 * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised. 00165 */ 00166 void saveGlobalMapForMATLAB( 00167 const std::string &filName, 00168 const THypothesisID &hypothesisID, 00169 const CHMHMapNode::TNodeID &idReferenceNode ) const; 00170 00171 00172 /** The Dijkstra algorithm for finding the shortest path between a pair of nodes. 00173 * \return The sequence of arcs connecting the nodes.It will be empty if no path is found or when the starting and ending node coincide. 00174 */ 00175 void findPathBetweenNodes( 00176 const CHMHMapNode::TNodeID &nodeFrom, 00177 const CHMHMapNode::TNodeID &nodeTo, 00178 const THypothesisID &hypothesisID, 00179 TArcList &out_path, 00180 bool direction=false) const; 00181 00182 00183 /** Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Area"'s nodes. 00184 * \exception std::exception If there is not enought information in arcs to compute the PDF 00185 * \sa computeGloballyConsistentNodeCoordinates 00186 */ 00187 void computeCoordinatesTransformationBetweenNodes( 00188 const CHMHMapNode::TNodeID &nodeFrom, 00189 const CHMHMapNode::TNodeID &nodeTo, 00190 CPose3DPDFParticles &posePDF, 00191 const THypothesisID &hypothesisID, 00192 unsigned int particlesCount = 100, 00193 float additionalNoiseXYratio = 0.02, 00194 float additionalNoisePhiRad = DEG2RAD(0.1) 00195 ) const; 00196 00197 /** Computes the probability [0,1] of two areas' gridmaps to "match" (loop closure), according to the grid maps and pose uncertainty from information in arcs (uses a Monte Carlo aproximation) 00198 * If there is not enough information or a robust estimation cannot be found, there will not be particles in "estimatedRelativePose". 00199 */ 00200 float computeMatchProbabilityBetweenNodes( 00201 const CHMHMapNode::TNodeID &nodeFrom, 00202 const CHMHMapNode::TNodeID &nodeTo, 00203 float &maxMatchProb, 00204 CPose3DPDFSOG &estimatedRelativePose, 00205 const THypothesisID &hypothesisID, 00206 unsigned int monteCarloSamplesPose = 300 00207 ); 00208 00209 /** Returns all the arcs between a pair of nodes: 00210 */ 00211 void findArcsBetweenNodes( 00212 const CHMHMapNode::TNodeID &node1, 00213 const CHMHMapNode::TNodeID &node2, 00214 const THypothesisID &hypothesisID, 00215 TArcList &out_listArcs ) const; 00216 00217 /** Returns the arcs between a pair of nodes of a given type. 00218 */ 00219 void findArcsOfTypeBetweenNodes( 00220 const CHMHMapNode::TNodeID &node1id, 00221 const CHMHMapNode::TNodeID &node2id, 00222 const THypothesisID &hypothesisID, 00223 const std::string &arcType, 00224 TArcList &ret) const; 00225 00226 /** Returns the first arc between a pair of nodes of a given type, and if it is in the opposite direction. 00227 * \return The arc, or NULL if not found. 00228 */ 00229 CHMHMapArcPtr findArcOfTypeBetweenNodes( 00230 const CHMHMapNode::TNodeID &node1id, 00231 const CHMHMapNode::TNodeID &node2id, 00232 const THypothesisID &hypothesisID, 00233 const std::string &arcType, 00234 bool &isInverted ) const; 00235 00236 00237 /** Returns wether two nodes are "neightbour", i.e. have a direct arc between them 00238 */ 00239 bool areNodesNeightbour( 00240 const CHMHMapNode::TNodeID &node1, 00241 const CHMHMapNode::TNodeID &node2, 00242 const THypothesisID &hypothesisID, 00243 const char *requiredAnnotation=NULL ) const; 00244 00245 /** This methods implements a Lu&Milios-like globally optimal estimation for the global coordinates of all the nodes in the graph according to all available arcs with relative pose information. 00246 * Global coordinates will be computed relative to the node "idReferenceNode". 00247 * \exception std::exception If there is any node without a pose arc, invalid (non invertible) matrixes, etc... 00248 * \sa computeCoordinatesTransformationBetweenNodes 00249 */ 00250 void computeGloballyConsistentNodeCoordinates( 00251 std::map<CHMHMapNode::TNodeID,CPose3DPDFGaussian, std::less<CHMHMapNode::TNodeID>, Eigen::aligned_allocator<std::pair<const CHMHMapNode::TNodeID,CPose3DPDFGaussian> > > &nodePoses, 00252 const CHMHMapNode::TNodeID &idReferenceNode, 00253 const THypothesisID &hypothesisID, 00254 const unsigned int &numberOfIterations = 2) const; 00255 00256 /** Returns a 3D scene reconstruction of the hierarchical map. 00257 * See "computeGloballyConsistentNodeCoordinates" for the meaning of "numberOfIterationsForOptimalGlobalPoses" 00258 */ 00259 void getAs3DScene( 00260 COpenGLScene &outScene, 00261 const CHMHMapNode::TNodeID &idReferenceNode, 00262 const THypothesisID &hypothesisID, 00263 const unsigned int &numberOfIterationsForOptimalGlobalPoses = 5, 00264 const bool &showRobotPoseIDs = true 00265 ) const; 00266 00267 /** Return a textual description of the whole graph */ 00268 void dumpAsText(utils::CStringList &s) const; 00269 00270 00271 00272 /** Computes the probability [0,1] of two areas' gridmaps to overlap, via a Monte Carlo aproximation. 00273 * \exception std::exception If there is not enought information in arcs, etc... 00274 * \param margin_to_substract In meters, the area of each gridmap is "eroded" this amount to compensate the area in excess usually found in gridmaps. 00275 */ 00276 double computeOverlapProbabilityBetweenNodes( 00277 const CHMHMapNode::TNodeID &nodeFrom, 00278 const CHMHMapNode::TNodeID &nodeTo, 00279 const THypothesisID &hypothesisID, 00280 const size_t &monteCarloSamples = 100, 00281 const float margin_to_substract = 6 00282 ) const ; 00283 00284 protected: 00285 00286 }; // End of class def. 00287 } 00288 } 00289 00290 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011 |