Main MRPT website > C++ reference for MRPT 1.4.0
CHierarchicalMapMHPartition.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CHierarchicalMapMHPartition_H
10 #define CHierarchicalMapMHPartition_H
11 
15 
20 
21 #include <map>
22 
23 namespace mrpt
24 {
25  namespace poses { class CPose3DPDFParticles; }
26 
27  namespace hmtslam
28  {
29  /** Represents a set of nodes and arcs, posibly only a part of the whole hierarchical, multi-hypothesis map.
30  * A usar will never create an instance of this class, rather it will employ CHierarchicalMHMap.
31  * \sa CHierarchicalMHMap, CHMHMapArc, CHMHMapNode
32  * \ingroup mrpt_hmtslam_grp
33  */
35  {
36  protected:
37  /** The internal list of nodes and arcs in the whole hierarchical model.
38  * The objects must be deleted only in the CHierarchicalMap class, not in partitions only objects.
39  */
42 
43  public:
44 
47 
48  /** Returns an iterator to the first node in the graph. */
49  const_iterator begin() const { return m_nodes.begin(); }
50 
51  /** Returns an iterator to the first node in the graph. */
52  iterator begin() { return m_nodes.begin(); }
53 
54  /** Returns an iterator to the end of the list of nodes in the graph. */
55  const_iterator end() const { return m_nodes.end(); }
56 
57  /** Returns an iterator to the end of the list of nodes in the graph. */
58  iterator end() { return m_nodes.end(); }
59 
60 
61  CHierarchicalMapMHPartition() : m_nodes(), m_arcs()
62  { }
63 
64  /** A type that reprensents a sequence of node IDs
65  */
66  typedef std::vector<CHMHMapNode::TNodeID> TNodeIDsList;
67 
68  /** Returns the number of nodes in the partition:
69  */
70  size_t nodeCount() const;
71 
72  /** Returns the number of arcs in the partition:
73  */
74  size_t arcCount() const;
75 
76  /** Returns the first node in the graph, or NULL if it does not exist.
77  * \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"
78  */
79  CHMHMapNodePtr getFirstNode();
80 
81  /** Returns the node with the given ID, or NULL if it does not exist.
82  * \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"
83  */
84  CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id);
85 
86  /** Returns the node with the given ID, or NULL if it does not exist.
87  * \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"
88  */
89  const CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id) const;
90 
91  /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist.
92  * \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"
93  */
94  CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID );
95 
96  /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist.
97  * \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"
98  */
99  const CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID) const;
100 
101  /** Returns a partition of this graph only with nodes at a given level in the hierarchy (0=ground level,1=parent level,etc)
102  * - The partition may be empty if no node fulfills the condition.
103  * - All arcs STARTING at each node from the partition will be added to the partition as well.
104  * - Levels in the hierarchy here stands for arcs of type "arcType_Belongs" only.
105  * \sa CHMHMapArc
106  */
107  //CHierarchicalMapMHPartition getPartitionByHiearchyLevel( unsigned int level );
108 
109  /** 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.
110  * ADDITIONAL NOTES:
111  * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
112  * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
113  */
115  const std::string &filName,
116  const CHMHMapNode::TNodeID &idReferenceNode,
117  const THypothesisID &hypothesisID) const;
118 
119  /** 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.
120  * ADDITIONAL NOTES:
121  * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
122  * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
123  */
125  const std::string &filName,
126  const CHMHMapNode::TNodeID &idReferenceNode,
127  const THypothesisID &hypothesisID,
128  float uncertaintyExagerationFactor = 1.0f,
129  bool drawArcs = false,
130  unsigned int numberOfIterationsForOptimalGlobalPoses = 4
131  ) const;
132 
133  /** Saves a MATLAB script that represents graphically the reconstructed "global map"
134  * ADDITIONAL NOTES:
135  * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
136  * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
137  */
139  const std::string &filName,
140  const THypothesisID &hypothesisID,
141  const CHMHMapNode::TNodeID &idReferenceNode ) const;
142 
143 
144  /** The Dijkstra algorithm for finding the shortest path between a pair of nodes.
145  * \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.
146  */
148  const CHMHMapNode::TNodeID &nodeFrom,
149  const CHMHMapNode::TNodeID &nodeTo,
150  const THypothesisID &hypothesisID,
151  TArcList &out_path,
152  bool direction=false) const;
153 
154 
155  /** Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Area"'s nodes.
156  * \exception std::exception If there is not enought information in arcs to compute the PDF
157  * \sa computeGloballyConsistentNodeCoordinates
158  */
160  const CHMHMapNode::TNodeID &nodeFrom,
161  const CHMHMapNode::TNodeID &nodeTo,
163  const THypothesisID &hypothesisID,
164  unsigned int particlesCount = 100,
165  float additionalNoiseXYratio = 0.02,
166  float additionalNoisePhiRad = mrpt::utils::DEG2RAD(0.1)
167  ) const;
168 
169  /** 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)
170  * If there is not enough information or a robust estimation cannot be found, there will not be particles in "estimatedRelativePose".
171  */
173  const CHMHMapNode::TNodeID &nodeFrom,
174  const CHMHMapNode::TNodeID &nodeTo,
175  float &maxMatchProb,
176  mrpt::poses::CPose3DPDFSOG &estimatedRelativePose,
177  const THypothesisID &hypothesisID,
178  unsigned int monteCarloSamplesPose = 300
179  );
180 
181  /** Returns all the arcs between a pair of nodes:
182  */
184  const CHMHMapNode::TNodeID &node1,
185  const CHMHMapNode::TNodeID &node2,
186  const THypothesisID &hypothesisID,
187  TArcList &out_listArcs ) const;
188 
189  /** Returns the arcs between a pair of nodes of a given type.
190  */
192  const CHMHMapNode::TNodeID &node1id,
193  const CHMHMapNode::TNodeID &node2id,
194  const THypothesisID &hypothesisID,
195  const std::string &arcType,
196  TArcList &ret) const;
197 
198  /** Returns the first arc between a pair of nodes of a given type, and if it is in the opposite direction.
199  * \return The arc, or NULL if not found.
200  */
202  const CHMHMapNode::TNodeID &node1id,
203  const CHMHMapNode::TNodeID &node2id,
204  const THypothesisID &hypothesisID,
205  const std::string &arcType,
206  bool &isInverted ) const;
207 
208 
209  /** Returns whether two nodes are "neightbour", i.e. have a direct arc between them */
211  const CHMHMapNode::TNodeID &node1,
212  const CHMHMapNode::TNodeID &node2,
213  const THypothesisID &hypothesisID,
214  const char *requiredAnnotation=NULL ) const;
215 
216  /** 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.
217  * Global coordinates will be computed relative to the node "idReferenceNode".
218  * \exception std::exception If there is any node without a pose arc, invalid (non invertible) matrixes, etc...
219  * \sa computeCoordinatesTransformationBetweenNodes
220  */
222  std::map<CHMHMapNode::TNodeID,mrpt::poses::CPose3DPDFGaussian, std::less<CHMHMapNode::TNodeID>, Eigen::aligned_allocator<std::pair<const CHMHMapNode::TNodeID,mrpt::poses::CPose3DPDFGaussian> > > &nodePoses,
223  const CHMHMapNode::TNodeID &idReferenceNode,
224  const THypothesisID &hypothesisID,
225  const unsigned int &numberOfIterations = 2) const;
226 
227  /** Returns a 3D scene reconstruction of the hierarchical map.
228  * See "computeGloballyConsistentNodeCoordinates" for the meaning of "numberOfIterationsForOptimalGlobalPoses"
229  */
231  mrpt::opengl::COpenGLScene &outScene,
232  const CHMHMapNode::TNodeID &idReferenceNode,
233  const THypothesisID &hypothesisID,
234  const unsigned int &numberOfIterationsForOptimalGlobalPoses = 5,
235  const bool &showRobotPoseIDs = true
236  ) const;
237 
238  /** Return a textual description of the whole graph */
240 
241 
242 
243  /** Computes the probability [0,1] of two areas' gridmaps to overlap, via a Monte Carlo aproximation.
244  * \exception std::exception If there is not enought information in arcs, etc...
245  * \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.
246  */
248  const CHMHMapNode::TNodeID &nodeFrom,
249  const CHMHMapNode::TNodeID &nodeTo,
250  const THypothesisID &hypothesisID,
251  const size_t &monteCarloSamples = 100,
252  const float margin_to_substract = 6
253  ) const ;
254 
255  protected:
256 
257  }; // End of class def.
258  }
259 }
260 
261 #endif
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
Represents a set of nodes and arcs, posibly only a part of the whole hierarchical,...
void findArcsOfTypeBetweenNodes(const CHMHMapNode::TNodeID &node1id, const CHMHMapNode::TNodeID &node2id, const THypothesisID &hypothesisID, const std::string &arcType, TArcList &ret) const
Returns the arcs between a pair of nodes of a given type.
iterator begin()
Returns an iterator to the first node in the graph.
void saveAreasDiagramWithEllipsedForMATLAB(const std::string &filName, const CHMHMapNode::TNodeID &idReferenceNode, const THypothesisID &hypothesisID, float uncertaintyExagerationFactor=1.0f, bool drawArcs=false, unsigned int numberOfIterationsForOptimalGlobalPoses=4) const
Saves a MATLAB script that represents graphically the nodes with type="Area" in this hierarchical-map...
iterator end()
Returns an iterator to the end of the list of nodes in the graph.
const CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id) const
Returns the node with the given ID, or NULL if it does not exist.
bool areNodesNeightbour(const CHMHMapNode::TNodeID &node1, const CHMHMapNode::TNodeID &node2, const THypothesisID &hypothesisID, const char *requiredAnnotation=NULL) const
Returns whether two nodes are "neightbour", i.e.
void computeCoordinatesTransformationBetweenNodes(const CHMHMapNode::TNodeID &nodeFrom, const CHMHMapNode::TNodeID &nodeTo, mrpt::poses::CPose3DPDFParticles &posePDF, const THypothesisID &hypothesisID, unsigned int particlesCount=100, float additionalNoiseXYratio=0.02, float additionalNoisePhiRad=mrpt::utils::DEG2RAD(0.1)) const
Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Ar...
size_t arcCount() const
Returns the number of arcs in the partition:
void findPathBetweenNodes(const CHMHMapNode::TNodeID &nodeFrom, const CHMHMapNode::TNodeID &nodeTo, const THypothesisID &hypothesisID, TArcList &out_path, bool direction=false) const
The Dijkstra algorithm for finding the shortest path between a pair of nodes.
void saveAreasDiagramForMATLAB(const std::string &filName, const CHMHMapNode::TNodeID &idReferenceNode, const THypothesisID &hypothesisID) const
Returns a partition of this graph only with nodes at a given level in the hierarchy (0=ground level,...
const_iterator begin() const
Returns an iterator to the first node in the graph.
void findArcsBetweenNodes(const CHMHMapNode::TNodeID &node1, const CHMHMapNode::TNodeID &node2, const THypothesisID &hypothesisID, TArcList &out_listArcs) const
Returns all the arcs between a pair of nodes:
const_iterator end() const
Returns an iterator to the end of the list of nodes in the graph.
double computeOverlapProbabilityBetweenNodes(const CHMHMapNode::TNodeID &nodeFrom, const CHMHMapNode::TNodeID &nodeTo, const THypothesisID &hypothesisID, const size_t &monteCarloSamples=100, const float margin_to_substract=6) const
Computes the probability [0,1] of two areas' gridmaps to overlap, via a Monte Carlo aproximation.
TNodeList m_nodes
The internal list of nodes and arcs in the whole hierarchical model.
const CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID) const
Returns the node with the given label (case insensitive) for some given hypothesis ID,...
CHMHMapArcPtr findArcOfTypeBetweenNodes(const CHMHMapNode::TNodeID &node1id, const CHMHMapNode::TNodeID &node2id, const THypothesisID &hypothesisID, const std::string &arcType, bool &isInverted) const
Returns the first arc between a pair of nodes of a given type, and if it is in the opposite direction...
void computeGloballyConsistentNodeCoordinates(std::map< CHMHMapNode::TNodeID, mrpt::poses::CPose3DPDFGaussian, std::less< CHMHMapNode::TNodeID >, Eigen::aligned_allocator< std::pair< const CHMHMapNode::TNodeID, mrpt::poses::CPose3DPDFGaussian > > > &nodePoses, const CHMHMapNode::TNodeID &idReferenceNode, const THypothesisID &hypothesisID, const unsigned int &numberOfIterations=2) const
This methods implements a Lu&Milios-like globally optimal estimation for the global coordinates of al...
void dumpAsText(utils::CStringList &s) const
Return a textual description of the whole graph.
void saveGlobalMapForMATLAB(const std::string &filName, const THypothesisID &hypothesisID, const CHMHMapNode::TNodeID &idReferenceNode) const
Saves a MATLAB script that represents graphically the reconstructed "global map" ADDITIONAL NOTES:
CHMHMapNodePtr getFirstNode()
Returns the first node in the graph, or NULL if it does not exist.
void getAs3DScene(mrpt::opengl::COpenGLScene &outScene, const CHMHMapNode::TNodeID &idReferenceNode, const THypothesisID &hypothesisID, const unsigned int &numberOfIterationsForOptimalGlobalPoses=5, const bool &showRobotPoseIDs=true) const
Returns a 3D scene reconstruction of the hierarchical map.
CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id)
Returns the node with the given ID, or NULL if it does not exist.
float computeMatchProbabilityBetweenNodes(const CHMHMapNode::TNodeID &nodeFrom, const CHMHMapNode::TNodeID &nodeTo, float &maxMatchProb, mrpt::poses::CPose3DPDFSOG &estimatedRelativePose, const THypothesisID &hypothesisID, unsigned int monteCarloSamplesPose=300)
Computes the probability [0,1] of two areas' gridmaps to "match" (loop closure), according to the gri...
CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID)
Returns the node with the given label (case insensitive) for some given hypothesis ID,...
size_t nodeCount() const
Returns the number of nodes in the partition:
std::vector< CHMHMapNode::TNodeID > TNodeIDsList
A type that reprensents a sequence of node IDs.
A class for storing a sequence of arcs (a path).
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:50
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose .
Definition: CPose3DPDFSOG.h:35
This base class provides a common printf-like method to send debug information to std::cout,...
A class for storing a list of text lines.
Definition: CStringList.h:33
Scalar * iterator
Definition: eigen_plugins.h:23
const Scalar * const_iterator
Definition: eigen_plugins.h:24
#define HMTSLAM_IMPEXP
std::map< CHMHMapNode::TNodeID, CHMHMapNodePtr > TNodeList
A map between node IDs and nodes (used in HMT-SLAM).
Definition: CHMHMapNode.h:148
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:69
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 03:56:21 UTC 2022