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
23namespace 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
45 typedef TNodeList::iterator iterator;
46 typedef TNodeList::const_iterator const_iterator;
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 */
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 */
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 */
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 */
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
#define HMTSLAM_IMPEXP
std::map< CHMHMapNode::TNodeID, CHMHMapNodePtr > TNodeList
A map between node IDs and nodes (used in HMT-SLAM).
Definition: CHMHMapNode.h:148
struct HMTSLAM_IMPEXP CHMHMapArcPtr
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
struct HMTSLAM_IMPEXP CHMHMapNodePtr
Definition: CHMHMapNode.h:28
class BASE_IMPEXP CPose3DPDFParticles
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.6 for MRPT 1.4.0 SVN: at Wed Mar 22 06:08:57 UTC 2023