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