Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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
26 {
28 }
29 
30 namespace hmtslam
31 {
32 /** Represents a set of nodes and arcs, posibly only a part of the whole
33  * hierarchical, multi-hypothesis map.
34  * A usar will never create an instance of this class, rather it will employ
35  * CHierarchicalMHMap.
36  * \sa CHierarchicalMHMap, CHMHMapArc, CHMHMapNode
37  * \ingroup mrpt_hmtslam_grp
38  */
40 {
41  protected:
42  /** The internal list of nodes and arcs in the whole hierarchical model.
43  * The objects must be deleted only in the CHierarchicalMap class, not in
44  * partitions only objects.
45  */
48 
49  public:
52 
53  /** Returns an iterator to the first node in the graph. */
54  const_iterator begin() const { return m_nodes.begin(); }
55  /** Returns an iterator to the first node in the graph. */
56  iterator begin() { return m_nodes.begin(); }
57  /** Returns an iterator to the end of the list of nodes in the graph. */
58  const_iterator end() const { return m_nodes.end(); }
59  /** Returns an iterator to the end of the list of nodes in the graph. */
60  iterator end() { return m_nodes.end(); }
62  /** A type that reprensents a sequence of node IDs
63  */
64  using TNodeIDsList = std::vector<CHMHMapNode::TNodeID>;
65 
66  /** Returns the number of nodes in the partition:
67  */
68  size_t nodeCount() const;
69 
70  /** Returns the number of arcs in the partition:
71  */
72  size_t arcCount() const;
73 
74  /** Returns the first node in the graph, or nullptr if it does not exist.
75  * \return A pointer to the object. DO NOT DELETE this object, if you want
76  * to modify it in someway, first obtain a copy by invoking
77  * "CSerializable::duplicate"
78  */
80 
81  /** Returns the node with the given ID, or nullptr if it does not exist.
82  * \return A pointer to the object. DO NOT DELETE this object, if you want
83  * to modify it in someway, first obtain a copy by invoking
84  * "CSerializable::duplicate"
85  */
87 
88  /** Returns the node with the given ID, or nullptr if it does not exist.
89  * \return A pointer to the object. DO NOT DELETE this object, if you want
90  * to modify it in someway, first obtain a copy by invoking
91  * "CSerializable::duplicate"
92  */
94 
95  /** Returns the node with the given label (case insensitive) for some given
96  * hypothesis ID, or nullptr if it does not exist.
97  * \return A pointer to the object. DO NOT DELETE this object, if you want
98  * to modify it in someway, first obtain a copy by invoking
99  * "CSerializable::duplicate"
100  */
102  const std::string& label, const THypothesisID& hypothesisID);
103 
104  /** Returns the node with the given label (case insensitive) for some given
105  * hypothesis ID, or nullptr if it does not exist.
106  * \return A pointer to the object. DO NOT DELETE this object, if you want
107  * to modify it in someway, first obtain a copy by invoking
108  * "CSerializable::duplicate"
109  */
111  const std::string& label, const THypothesisID& hypothesisID) const;
112 
113  /** Returns a partition of this graph only with nodes at a given level in
114  *the hierarchy (0=ground level,1=parent level,etc)
115  * - The partition may be empty if no node fulfills the condition.
116  * - All arcs STARTING at each node from the partition will be added to
117  *the partition as well.
118  * - Levels in the hierarchy here stands for arcs of type
119  *"arcType_Belongs" only.
120  * \sa CHMHMapArc
121  */
122  // CHierarchicalMapMHPartition getPartitionByHiearchyLevel( unsigned int
123  // level );
124 
125  /** Saves a MATLAB script that represents graphically the nodes with
126  *<i>type</i>="Area" in this hierarchical-map(partition), using the stated
127  *node as global coordinates reference.
128  * ADDITIONAL NOTES:
129  * - Coordinates are computed simply as the mean value of the first arc
130  *with an annotation "RelativePose", added to the pose of the original
131  *node.
132  * - If the coordinates of any node can not be computed (no arcs,...), an
133  *exception will be raised.
134  */
136  const std::string& filName, const CHMHMapNode::TNodeID& idReferenceNode,
137  const THypothesisID& hypothesisID) const;
138 
139  /** Saves a MATLAB script that represents graphically the nodes with
140  *<i>type</i>="Area" in this hierarchical-map(partition), using the stated
141  *node as global coordinates reference, and drawing the ellipses of the
142  *localization uncertainty for each node.
143  * ADDITIONAL NOTES:
144  * - Coordinates are computed simply as the mean value of the first arc
145  *with an annotation "RelativePose", added to the pose of the original
146  *node.
147  * - If the coordinates of any node can not be computed (no arcs,...), an
148  *exception will be raised.
149  */
151  const std::string& filName, const CHMHMapNode::TNodeID& idReferenceNode,
152  const THypothesisID& hypothesisID,
153  float uncertaintyExagerationFactor = 1.0f, bool drawArcs = false,
154  unsigned int numberOfIterationsForOptimalGlobalPoses = 4) const;
155 
156  /** Saves a MATLAB script that represents graphically the reconstructed
157  *"global map"
158  * ADDITIONAL NOTES:
159  * - Coordinates are computed simply as the mean value of the first arc
160  *with an annotation "RelativePose", added to the pose of the original
161  *node.
162  * - If the coordinates of any node can not be computed (no arcs,...), an
163  *exception will be raised.
164  */
166  const std::string& filName, const THypothesisID& hypothesisID,
167  const CHMHMapNode::TNodeID& idReferenceNode) const;
168 
169  /** The Dijkstra algorithm for finding the shortest path between a pair of
170  * nodes.
171  * \return The sequence of arcs connecting the nodes.It will be empty if no
172  * path is found or when the starting and ending node coincide.
173  */
175  const CHMHMapNode::TNodeID& nodeFrom,
176  const CHMHMapNode::TNodeID& nodeTo, const THypothesisID& hypothesisID,
177  TArcList& out_path, bool direction = false) const;
178 
179  /** Draw a number of samples according to the PDF of the coordinates
180  * transformation between a pair of "Area"'s nodes.
181  * \exception std::exception If there is not enought information in arcs to
182  * compute the PDF
183  * \sa computeGloballyConsistentNodeCoordinates
184  */
186  const CHMHMapNode::TNodeID& nodeFrom,
187  const CHMHMapNode::TNodeID& nodeTo,
189  const THypothesisID& hypothesisID, unsigned int particlesCount = 100,
190  float additionalNoiseXYratio = 0.02,
191  float additionalNoisePhiRad = mrpt::DEG2RAD(0.1)) const;
192 
193  /** Computes the probability [0,1] of two areas' gridmaps to "match" (loop
194  * closure), according to the grid maps and pose uncertainty from
195  * information in arcs (uses a Monte Carlo aproximation)
196  * If there is not enough information or a robust estimation cannot be
197  * found, there will not be particles in "estimatedRelativePose".
198  */
200  const CHMHMapNode::TNodeID& nodeFrom,
201  const CHMHMapNode::TNodeID& nodeTo, float& maxMatchProb,
202  mrpt::poses::CPose3DPDFSOG& estimatedRelativePose,
203  const THypothesisID& hypothesisID,
204  unsigned int monteCarloSamplesPose = 300);
205 
206  /** Returns all the arcs between a pair of nodes:
207  */
209  const CHMHMapNode::TNodeID& node1, const CHMHMapNode::TNodeID& node2,
210  const THypothesisID& hypothesisID, TArcList& out_listArcs) const;
211 
212  /** Returns the arcs between a pair of nodes of a given type.
213  */
215  const CHMHMapNode::TNodeID& node1id,
216  const CHMHMapNode::TNodeID& node2id, const THypothesisID& hypothesisID,
217  const std::string& arcType, TArcList& ret) const;
218 
219  /** Returns the first arc between a pair of nodes of a given type, and if it
220  * is in the opposite direction.
221  * \return The arc, or nullptr if not found.
222  */
224  const CHMHMapNode::TNodeID& node1id,
225  const CHMHMapNode::TNodeID& node2id, const THypothesisID& hypothesisID,
226  const std::string& arcType, bool& isInverted) const;
227 
228  /** Returns whether two nodes are "neightbour", i.e. have a direct arc
229  * between them */
230  bool areNodesNeightbour(
231  const CHMHMapNode::TNodeID& node1, const CHMHMapNode::TNodeID& node2,
232  const THypothesisID& hypothesisID,
233  const char* requiredAnnotation = nullptr) const;
234 
235  /** This methods implements a Lu&Milios-like globally optimal estimation for
236  * the global coordinates of all the nodes in the graph according to all
237  * available arcs with relative pose information.
238  * Global coordinates will be computed relative to the node
239  * "idReferenceNode".
240  * \exception std::exception If there is any node without a pose arc,
241  * invalid (non invertible) matrixes, etc...
242  * \sa computeCoordinatesTransformationBetweenNodes
243  */
246  std::less<CHMHMapNode::TNodeID>,
247  Eigen::aligned_allocator<
248  std::pair<const CHMHMapNode::TNodeID,
249  mrpt::poses::CPose3DPDFGaussian>>>& nodePoses,
250  const CHMHMapNode::TNodeID& idReferenceNode,
251  const THypothesisID& hypothesisID,
252  const unsigned int& numberOfIterations = 2) const;
253 
254  /** Returns a 3D scene reconstruction of the hierarchical map.
255  * See "computeGloballyConsistentNodeCoordinates" for the meaning of
256  * "numberOfIterationsForOptimalGlobalPoses"
257  */
258  void getAs3DScene(
259  mrpt::opengl::COpenGLScene& outScene,
260  const CHMHMapNode::TNodeID& idReferenceNode,
261  const THypothesisID& hypothesisID,
262  const unsigned int& numberOfIterationsForOptimalGlobalPoses = 5,
263  const bool& showRobotPoseIDs = true) const;
264 
265  /** Return a textual description of the whole graph */
266  void dumpAsText(std::vector<std::string>& s) const;
267 
268  /** Computes the probability [0,1] of two areas' gridmaps to overlap, via a
269  * Monte Carlo aproximation.
270  * \exception std::exception If there is not enought information in arcs,
271  * etc...
272  * \param margin_to_substract In meters, the area of each gridmap is
273  * "eroded" this amount to compensate the area in excess usually found in
274  * gridmaps.
275  */
277  const CHMHMapNode::TNodeID& nodeFrom,
278  const CHMHMapNode::TNodeID& nodeTo, const THypothesisID& hypothesisID,
279  const size_t& monteCarloSamples = 100,
280  const float margin_to_substract = 6) const;
281 
282  protected:
283 }; // End of class def.
284 }
285 }
286 
287 #endif
Scalar * iterator
Definition: eigen_plugins.h:26
std::map< CHMHMapNode::TNodeID, std::shared_ptr< CHMHMapNode > > TNodeList
A map between node IDs and nodes (used in HMT-SLAM).
Definition: CHMHMapNode.h:144
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&#39; gridmaps to "match" (loop closure), according to the gri...
CHMHMapArc::Ptr 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...
Represents a set of nodes and arcs, posibly only a part of the whole hierarchical, multi-hypothesis map.
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:47
double DEG2RAD(const double x)
Degrees to radians.
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:34
CHMHMapNode::Ptr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID)
Returns the node with the given label (case insensitive) for some given hypothesis ID...
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...
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: ...
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.
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:
GLdouble s
Definition: glext.h:3676
size_t arcCount() const
Returns the number of arcs in the partition:
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::DEG2RAD(0.1)) const
Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Ar...
iterator begin()
Returns an iterator to the first node in the graph.
Versatile class for consistent logging and management of output messages.
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.
iterator end()
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&#39; gridmaps to overlap, via a Monte Carlo aproximation...
std::vector< CHMHMapNode::TNodeID > TNodeIDsList
A type that reprensents a sequence of node IDs.
GLsizei const GLchar ** string
Definition: glext.h:4101
size_t nodeCount() const
Returns the number of nodes in the partition:
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CHMHMapNode::Ptr getFirstNode()
Returns the first node in the graph, or nullptr if it does not exist.
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:59
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
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.
const_iterator begin() const
Returns an iterator to the first node in the graph.
void dumpAsText(std::vector< std::string > &s) const
Return a textual description of the whole graph.
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...
bool areNodesNeightbour(const CHMHMapNode::TNodeID &node1, const CHMHMapNode::TNodeID &node2, const THypothesisID &hypothesisID, const char *requiredAnnotation=nullptr) const
Returns whether two nodes are "neightbour", i.e.
TNodeList m_nodes
The internal list of nodes and arcs in the whole hierarchical model.
const Scalar * const_iterator
Definition: eigen_plugins.h:27
A class for storing a sequence of arcs (a path).
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...
CHMHMapNode::Ptr getNodeByID(CHMHMapNode::TNodeID id)
Returns the node with the given ID, or nullptr if it does not exist.
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.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019