MRPT  1.9.9
CHierarchicalMapMHPartition.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
14 
19 
20 #include <map>
21 
22 namespace mrpt
23 {
24 namespace poses
25 {
27 }
28 
29 namespace hmtslam
30 {
31 /** Represents a set of nodes and arcs, posibly only a part of the whole
32  * hierarchical, multi-hypothesis map.
33  * A usar will never create an instance of this class, rather it will employ
34  * CHierarchicalMHMap.
35  * \sa CHierarchicalMHMap, CHMHMapArc, CHMHMapNode
36  * \ingroup mrpt_hmtslam_grp
37  */
39 {
40  protected:
41  /** The internal list of nodes and arcs in the whole hierarchical model.
42  * The objects must be deleted only in the CHierarchicalMap class, not in
43  * partitions only objects.
44  */
47 
48  public:
51 
52  /** Returns an iterator to the first node in the graph. */
53  const_iterator begin() const { return m_nodes.begin(); }
54  /** Returns an iterator to the first node in the graph. */
55  iterator begin() { return m_nodes.begin(); }
56  /** Returns an iterator to the end of the list of nodes in the graph. */
57  const_iterator end() const { return m_nodes.end(); }
58  /** Returns an iterator to the end of the list of nodes in the graph. */
59  iterator end() { return m_nodes.end(); }
61  /** A type that reprensents a sequence of node IDs
62  */
63  using TNodeIDsList = std::vector<CHMHMapNode::TNodeID>;
64 
65  /** Returns the number of nodes in the partition:
66  */
67  size_t nodeCount() const;
68 
69  /** Returns the number of arcs in the partition:
70  */
71  size_t arcCount() const;
72 
73  /** Returns the first node in the graph, or nullptr if it does not exist.
74  * \return A pointer to the object. DO NOT DELETE this object, if you want
75  * to modify it in someway, first obtain a copy by invoking
76  * "CSerializable::duplicate"
77  */
79 
80  /** Returns the node with the given ID, or nullptr if it does not exist.
81  * \return A pointer to the object. DO NOT DELETE this object, if you want
82  * to modify it in someway, first obtain a copy by invoking
83  * "CSerializable::duplicate"
84  */
86 
87  /** Returns the node with the given ID, or nullptr if it does not exist.
88  * \return A pointer to the object. DO NOT DELETE this object, if you want
89  * to modify it in someway, first obtain a copy by invoking
90  * "CSerializable::duplicate"
91  */
93 
94  /** Returns the node with the given label (case insensitive) for some given
95  * hypothesis ID, or nullptr if it does not exist.
96  * \return A pointer to the object. DO NOT DELETE this object, if you want
97  * to modify it in someway, first obtain a copy by invoking
98  * "CSerializable::duplicate"
99  */
101  const std::string& label, const THypothesisID& hypothesisID);
102 
103  /** Returns the node with the given label (case insensitive) for some given
104  * hypothesis ID, or nullptr if it does not exist.
105  * \return A pointer to the object. DO NOT DELETE this object, if you want
106  * to modify it in someway, first obtain a copy by invoking
107  * "CSerializable::duplicate"
108  */
110  const std::string& label, const THypothesisID& hypothesisID) const;
111 
112  /** Returns a partition of this graph only with nodes at a given level in
113  *the hierarchy (0=ground level,1=parent level,etc)
114  * - The partition may be empty if no node fulfills the condition.
115  * - All arcs STARTING at each node from the partition will be added to
116  *the partition as well.
117  * - Levels in the hierarchy here stands for arcs of type
118  *"arcType_Belongs" only.
119  * \sa CHMHMapArc
120  */
121  // CHierarchicalMapMHPartition getPartitionByHiearchyLevel( unsigned int
122  // level );
123 
124  /** Saves a MATLAB script that represents graphically the nodes with
125  *<i>type</i>="Area" in this hierarchical-map(partition), using the stated
126  *node as global coordinates reference.
127  * ADDITIONAL NOTES:
128  * - Coordinates are computed simply as the mean value of the first arc
129  *with an annotation "RelativePose", added to the pose of the original
130  *node.
131  * - If the coordinates of any node can not be computed (no arcs,...), an
132  *exception will be raised.
133  */
135  const std::string& filName, const CHMHMapNode::TNodeID& idReferenceNode,
136  const THypothesisID& hypothesisID) const;
137 
138  /** Saves a MATLAB script that represents graphically the nodes with
139  *<i>type</i>="Area" in this hierarchical-map(partition), using the stated
140  *node as global coordinates reference, and drawing the ellipses of the
141  *localization uncertainty for each node.
142  * ADDITIONAL NOTES:
143  * - Coordinates are computed simply as the mean value of the first arc
144  *with an annotation "RelativePose", added to the pose of the original
145  *node.
146  * - If the coordinates of any node can not be computed (no arcs,...), an
147  *exception will be raised.
148  */
150  const std::string& filName, const CHMHMapNode::TNodeID& idReferenceNode,
151  const THypothesisID& hypothesisID,
152  float uncertaintyExagerationFactor = 1.0f, bool drawArcs = false,
153  unsigned int numberOfIterationsForOptimalGlobalPoses = 4) const;
154 
155  /** Saves a MATLAB script that represents graphically the reconstructed
156  *"global map"
157  * ADDITIONAL NOTES:
158  * - Coordinates are computed simply as the mean value of the first arc
159  *with an annotation "RelativePose", added to the pose of the original
160  *node.
161  * - If the coordinates of any node can not be computed (no arcs,...), an
162  *exception will be raised.
163  */
165  const std::string& filName, const THypothesisID& hypothesisID,
166  const CHMHMapNode::TNodeID& idReferenceNode) const;
167 
168  /** The Dijkstra algorithm for finding the shortest path between a pair of
169  * nodes.
170  * \return The sequence of arcs connecting the nodes.It will be empty if no
171  * path is found or when the starting and ending node coincide.
172  */
174  const CHMHMapNode::TNodeID& nodeFrom,
175  const CHMHMapNode::TNodeID& nodeTo, const THypothesisID& hypothesisID,
176  TArcList& out_path, bool direction = false) const;
177 
178  /** Draw a number of samples according to the PDF of the coordinates
179  * transformation between a pair of "Area"'s nodes.
180  * \exception std::exception If there is not enought information in arcs to
181  * compute the PDF
182  * \sa computeGloballyConsistentNodeCoordinates
183  */
185  const CHMHMapNode::TNodeID& nodeFrom,
186  const CHMHMapNode::TNodeID& nodeTo,
188  const THypothesisID& hypothesisID, unsigned int particlesCount = 100,
189  float additionalNoiseXYratio = 0.02,
190  float additionalNoisePhiRad = mrpt::DEG2RAD(0.1)) const;
191 
192  /** Computes the probability [0,1] of two areas' gridmaps to "match" (loop
193  * closure), according to the grid maps and pose uncertainty from
194  * information in arcs (uses a Monte Carlo aproximation)
195  * If there is not enough information or a robust estimation cannot be
196  * found, there will not be particles in "estimatedRelativePose".
197  */
199  const CHMHMapNode::TNodeID& nodeFrom,
200  const CHMHMapNode::TNodeID& nodeTo, float& maxMatchProb,
201  mrpt::poses::CPose3DPDFSOG& estimatedRelativePose,
202  const THypothesisID& hypothesisID,
203  unsigned int monteCarloSamplesPose = 300);
204 
205  /** Returns all the arcs between a pair of nodes:
206  */
208  const CHMHMapNode::TNodeID& node1, const CHMHMapNode::TNodeID& node2,
209  const THypothesisID& hypothesisID, TArcList& out_listArcs) const;
210 
211  /** Returns the arcs between a pair of nodes of a given type.
212  */
214  const CHMHMapNode::TNodeID& node1id,
215  const CHMHMapNode::TNodeID& node2id, const THypothesisID& hypothesisID,
216  const std::string& arcType, TArcList& ret) const;
217 
218  /** Returns the first arc between a pair of nodes of a given type, and if it
219  * is in the opposite direction.
220  * \return The arc, or nullptr if not found.
221  */
223  const CHMHMapNode::TNodeID& node1id,
224  const CHMHMapNode::TNodeID& node2id, const THypothesisID& hypothesisID,
225  const std::string& arcType, bool& isInverted) const;
226 
227  /** Returns whether two nodes are "neightbour", i.e. have a direct arc
228  * between them */
229  bool areNodesNeightbour(
230  const CHMHMapNode::TNodeID& node1, const CHMHMapNode::TNodeID& node2,
231  const THypothesisID& hypothesisID,
232  const char* requiredAnnotation = nullptr) const;
233 
234  /** This methods implements a Lu&Milios-like globally optimal estimation for
235  * the global coordinates of all the nodes in the graph according to all
236  * available arcs with relative pose information.
237  * Global coordinates will be computed relative to the node
238  * "idReferenceNode".
239  * \exception std::exception If there is any node without a pose arc,
240  * invalid (non invertible) matrixes, etc...
241  * \sa computeCoordinatesTransformationBetweenNodes
242  */
244  std::map<
246  std::less<CHMHMapNode::TNodeID>,
247  Eigen::aligned_allocator<std::pair<
249  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 } // namespace hmtslam
285 } // namespace mrpt
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:141
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:44
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:32
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:3682
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:4116
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:58
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: 58f856185 Mon May 20 11:43:57 2019 +0200 at lun may 20 11:50:17 CEST 2019