Main MRPT website > C++ reference
MRPT logo

CHierarchicalMapMHPartition.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 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:40:17 UTC 2011