Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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  */
34  class HMTSLAM_IMPEXP CHierarchicalMapMHPartition : public mrpt::utils::COutputLogger
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  */
114  void saveAreasDiagramForMATLAB(
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  */
124  void saveAreasDiagramWithEllipsedForMATLAB(
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  */
138  void saveGlobalMapForMATLAB(
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  */
147  void findPathBetweenNodes(
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  */
159  void computeCoordinatesTransformationBetweenNodes(
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  */
172  float computeMatchProbabilityBetweenNodes(
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  */
183  void findArcsBetweenNodes(
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  */
191  void findArcsOfTypeBetweenNodes(
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  */
201  CHMHMapArcPtr findArcOfTypeBetweenNodes(
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 */
210  bool areNodesNeightbour(
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  */
221  void computeGloballyConsistentNodeCoordinates(
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  */
230  void getAs3DScene(
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 */
239  void dumpAsText(utils::CStringList &s) const;
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  */
247  double computeOverlapProbabilityBetweenNodes(
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
std::vector< CHMHMapNode::TNodeID > TNodeIDsList
A type that reprensents a sequence of node IDs.
Represents a set of nodes and arcs, posibly only a part of the whole hierarchical, multi-hypothesis map.
double DEG2RAD(const double x)
Degrees to radians.
Scalar * iterator
Definition: eigen_plugins.h:23
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:34
std::map< CHMHMapNode::TNodeID, CHMHMapNodePtr > TNodeList
A map between node IDs and nodes (used in HMT-SLAM).
Definition: CHMHMapNode.h:148
const Scalar * const_iterator
Definition: eigen_plugins.h:24
GLdouble s
Definition: glext.h:3602
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
A class for storing a list of text lines.
Definition: CStringList.h:32
iterator begin()
Returns an iterator to the first node in the graph.
iterator end()
Returns an iterator to the end of the list of nodes in the graph.
GLsizei const GLchar ** string
Definition: glext.h:3919
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:49
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
const_iterator begin() const
Returns an iterator to the first node in the graph.
TNodeList m_nodes
The internal list of nodes and arcs in the whole hierarchical model.
A class for storing a sequence of arcs (a path).
const_iterator end() const
Returns an iterator to the end of the list of nodes in the graph.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019