00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 * \ingroup mrpt_hmtslam_grp 00062 */ 00063 class HMTSLAM_IMPEXP CHierarchicalMapMHPartition : public mrpt::utils::CDebugOutputCapable 00064 { 00065 protected: 00066 /** The internal list of nodes and arcs in the whole hierarchical model. 00067 * The objects must be deleted only in the CHierarchicalMap class, not in partitions only objects. 00068 */ 00069 TNodeList m_nodes; 00070 TArcList m_arcs; 00071 00072 public: 00073 00074 typedef TNodeList::iterator iterator; 00075 typedef TNodeList::const_iterator const_iterator; 00076 00077 /** Returns an iterator to the first node in the graph. */ 00078 const_iterator begin() const { return m_nodes.begin(); } 00079 00080 /** Returns an iterator to the first node in the graph. */ 00081 iterator begin() { return m_nodes.begin(); } 00082 00083 /** Returns an iterator to the end of the list of nodes in the graph. */ 00084 const_iterator end() const { return m_nodes.end(); } 00085 00086 /** Returns an iterator to the end of the list of nodes in the graph. */ 00087 iterator end() { return m_nodes.end(); } 00088 00089 00090 CHierarchicalMapMHPartition() : m_nodes(), m_arcs() 00091 { } 00092 00093 /** A type that reprensents a sequence of node IDs 00094 */ 00095 typedef std::vector<CHMHMapNode::TNodeID> TNodeIDsList; 00096 00097 /** Returns the number of nodes in the partition: 00098 */ 00099 size_t nodeCount() const; 00100 00101 /** Returns the number of arcs in the partition: 00102 */ 00103 size_t arcCount() const; 00104 00105 /** Returns the first node in the graph, or NULL if it does not exist. 00106 * \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" 00107 */ 00108 CHMHMapNodePtr getFirstNode(); 00109 00110 /** Returns the node with the given ID, or NULL if it does not exist. 00111 * \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" 00112 */ 00113 CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id); 00114 00115 /** Returns the node with the given ID, or NULL if it does not exist. 00116 * \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" 00117 */ 00118 const CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id) const; 00119 00120 /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist. 00121 * \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" 00122 */ 00123 CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID ); 00124 00125 /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist. 00126 * \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" 00127 */ 00128 const CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID) const; 00129 00130 /** Returns a partition of this graph only with nodes at a given level in the hierarchy (0=ground level,1=parent level,etc) 00131 * - The partition may be empty if no node fulfills the condition. 00132 * - All arcs STARTING at each node from the partition will be added to the partition as well. 00133 * - Levels in the hierarchy here stands for arcs of type "arcType_Belongs" only. 00134 * \sa CHMHMapArc 00135 */ 00136 //CHierarchicalMapMHPartition getPartitionByHiearchyLevel( unsigned int level ); 00137 00138 /** 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. 00139 * ADDITIONAL NOTES: 00140 * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node. 00141 * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised. 00142 */ 00143 void saveAreasDiagramForMATLAB( 00144 const std::string &filName, 00145 const CHMHMapNode::TNodeID &idReferenceNode, 00146 const THypothesisID &hypothesisID) const; 00147 00148 /** 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. 00149 * ADDITIONAL NOTES: 00150 * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node. 00151 * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised. 00152 */ 00153 void saveAreasDiagramWithEllipsedForMATLAB( 00154 const std::string &filName, 00155 const CHMHMapNode::TNodeID &idReferenceNode, 00156 const THypothesisID &hypothesisID, 00157 float uncertaintyExagerationFactor = 1.0f, 00158 bool drawArcs = false, 00159 unsigned int numberOfIterationsForOptimalGlobalPoses = 4 00160 ) const; 00161 00162 /** Saves a MATLAB script that represents graphically the reconstructed "global map" 00163 * ADDITIONAL NOTES: 00164 * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node. 00165 * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised. 00166 */ 00167 void saveGlobalMapForMATLAB( 00168 const std::string &filName, 00169 const THypothesisID &hypothesisID, 00170 const CHMHMapNode::TNodeID &idReferenceNode ) const; 00171 00172 00173 /** The Dijkstra algorithm for finding the shortest path between a pair of nodes. 00174 * \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. 00175 */ 00176 void findPathBetweenNodes( 00177 const CHMHMapNode::TNodeID &nodeFrom, 00178 const CHMHMapNode::TNodeID &nodeTo, 00179 const THypothesisID &hypothesisID, 00180 TArcList &out_path, 00181 bool direction=false) const; 00182 00183 00184 /** Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Area"'s nodes. 00185 * \exception std::exception If there is not enought information in arcs to compute the PDF 00186 * \sa computeGloballyConsistentNodeCoordinates 00187 */ 00188 void computeCoordinatesTransformationBetweenNodes( 00189 const CHMHMapNode::TNodeID &nodeFrom, 00190 const CHMHMapNode::TNodeID &nodeTo, 00191 CPose3DPDFParticles &posePDF, 00192 const THypothesisID &hypothesisID, 00193 unsigned int particlesCount = 100, 00194 float additionalNoiseXYratio = 0.02, 00195 float additionalNoisePhiRad = DEG2RAD(0.1) 00196 ) const; 00197 00198 /** 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) 00199 * If there is not enough information or a robust estimation cannot be found, there will not be particles in "estimatedRelativePose". 00200 */ 00201 float computeMatchProbabilityBetweenNodes( 00202 const CHMHMapNode::TNodeID &nodeFrom, 00203 const CHMHMapNode::TNodeID &nodeTo, 00204 float &maxMatchProb, 00205 CPose3DPDFSOG &estimatedRelativePose, 00206 const THypothesisID &hypothesisID, 00207 unsigned int monteCarloSamplesPose = 300 00208 ); 00209 00210 /** Returns all the arcs between a pair of nodes: 00211 */ 00212 void findArcsBetweenNodes( 00213 const CHMHMapNode::TNodeID &node1, 00214 const CHMHMapNode::TNodeID &node2, 00215 const THypothesisID &hypothesisID, 00216 TArcList &out_listArcs ) const; 00217 00218 /** Returns the arcs between a pair of nodes of a given type. 00219 */ 00220 void findArcsOfTypeBetweenNodes( 00221 const CHMHMapNode::TNodeID &node1id, 00222 const CHMHMapNode::TNodeID &node2id, 00223 const THypothesisID &hypothesisID, 00224 const std::string &arcType, 00225 TArcList &ret) const; 00226 00227 /** Returns the first arc between a pair of nodes of a given type, and if it is in the opposite direction. 00228 * \return The arc, or NULL if not found. 00229 */ 00230 CHMHMapArcPtr findArcOfTypeBetweenNodes( 00231 const CHMHMapNode::TNodeID &node1id, 00232 const CHMHMapNode::TNodeID &node2id, 00233 const THypothesisID &hypothesisID, 00234 const std::string &arcType, 00235 bool &isInverted ) const; 00236 00237 00238 /** Returns wether two nodes are "neightbour", i.e. have a direct arc between them 00239 */ 00240 bool areNodesNeightbour( 00241 const CHMHMapNode::TNodeID &node1, 00242 const CHMHMapNode::TNodeID &node2, 00243 const THypothesisID &hypothesisID, 00244 const char *requiredAnnotation=NULL ) const; 00245 00246 /** 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. 00247 * Global coordinates will be computed relative to the node "idReferenceNode". 00248 * \exception std::exception If there is any node without a pose arc, invalid (non invertible) matrixes, etc... 00249 * \sa computeCoordinatesTransformationBetweenNodes 00250 */ 00251 void computeGloballyConsistentNodeCoordinates( 00252 std::map<CHMHMapNode::TNodeID,CPose3DPDFGaussian, std::less<CHMHMapNode::TNodeID>, Eigen::aligned_allocator<std::pair<const CHMHMapNode::TNodeID,CPose3DPDFGaussian> > > &nodePoses, 00253 const CHMHMapNode::TNodeID &idReferenceNode, 00254 const THypothesisID &hypothesisID, 00255 const unsigned int &numberOfIterations = 2) const; 00256 00257 /** Returns a 3D scene reconstruction of the hierarchical map. 00258 * See "computeGloballyConsistentNodeCoordinates" for the meaning of "numberOfIterationsForOptimalGlobalPoses" 00259 */ 00260 void getAs3DScene( 00261 COpenGLScene &outScene, 00262 const CHMHMapNode::TNodeID &idReferenceNode, 00263 const THypothesisID &hypothesisID, 00264 const unsigned int &numberOfIterationsForOptimalGlobalPoses = 5, 00265 const bool &showRobotPoseIDs = true 00266 ) const; 00267 00268 /** Return a textual description of the whole graph */ 00269 void dumpAsText(utils::CStringList &s) const; 00270 00271 00272 00273 /** Computes the probability [0,1] of two areas' gridmaps to overlap, via a Monte Carlo aproximation. 00274 * \exception std::exception If there is not enought information in arcs, etc... 00275 * \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. 00276 */ 00277 double computeOverlapProbabilityBetweenNodes( 00278 const CHMHMapNode::TNodeID &nodeFrom, 00279 const CHMHMapNode::TNodeID &nodeTo, 00280 const THypothesisID &hypothesisID, 00281 const size_t &monteCarloSamples = 100, 00282 const float margin_to_substract = 6 00283 ) const ; 00284 00285 protected: 00286 00287 }; // End of class def. 00288 } 00289 } 00290 00291 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |