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
mrpt::hmtslam::CHierarchicalMapMHPartition::nodeCount
size_t nodeCount() const
Returns the number of nodes in the partition:
Definition: CHierarchicalMapMHPartition.cpp:41
mrpt::poses::CPose3DPDFSOG
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose .
Definition: CPose3DPDFSOG.h:32
mrpt::hmtslam::CHierarchicalMapMHPartition::end
iterator end()
Returns an iterator to the end of the list of nodes in the graph.
Definition: CHierarchicalMapMHPartition.h:57
mrpt::hmtslam::CHierarchicalMapMHPartition::begin
iterator begin()
Returns an iterator to the first node in the graph.
Definition: CHierarchicalMapMHPartition.h:53
mrpt::hmtslam::CHMHMapNode::Ptr
std::shared_ptr< mrpt::hmtslam ::CHMHMapNode > Ptr
Definition: CHMHMapNode.h:39
mrpt::hmtslam::CHierarchicalMapMHPartition::findPathBetweenNodes
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.
Definition: CHierarchicalMapMHPartition.cpp:726
opengl_frwds.h
CHMHMapNode.h
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives.
Definition: COpenGLScene.h:56
mrpt::hmtslam::CHierarchicalMapMHPartition::TNodeIDsList
std::vector< CHMHMapNode::TNodeID > TNodeIDsList
A type that reprensents a sequence of node IDs.
Definition: CHierarchicalMapMHPartition.h:61
CPose3DPDFGaussian.h
mrpt::hmtslam::CHierarchicalMapMHPartition::begin
const_iterator begin() const
Returns an iterator to the first node in the graph.
Definition: CHierarchicalMapMHPartition.h:51
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::hmtslam::CHierarchicalMapMHPartition::saveAreasDiagramForMATLAB
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,...
Definition: CHierarchicalMapMHPartition.cpp:128
mrpt::hmtslam::CHierarchicalMapMHPartition::dumpAsText
void dumpAsText(std::vector< std::string > &s) const
Return a textual description of the whole graph.
Definition: CHierarchicalMapMHPartition.cpp:1370
mrpt::hmtslam::CHierarchicalMapMHPartition::findArcsOfTypeBetweenNodes
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.
Definition: CHierarchicalMapMHPartition.cpp:1029
CLocalMetricHypothesis.h
mrpt::hmtslam::CHierarchicalMapMHPartition::findArcOfTypeBetweenNodes
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...
Definition: CHierarchicalMapMHPartition.cpp:1507
mrpt::hmtslam::CHierarchicalMapMHPartition::computeOverlapProbabilityBetweenNodes
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' gridmaps to overlap, via a Monte Carlo aproximation.
Definition: CHierarchicalMapMHPartition.cpp:1541
mrpt::hmtslam::CHierarchicalMapMHPartition::iterator
TNodeList::iterator iterator
Definition: CHierarchicalMapMHPartition.h:47
COutputLogger.h
mrpt::hmtslam::CHierarchicalMapMHPartition::getNodeByID
CHMHMapNode::Ptr getNodeByID(CHMHMapNode::TNodeID id)
Returns the node with the given ID, or nullptr if it does not exist.
Definition: CHierarchicalMapMHPartition.cpp:49
mrpt::hmtslam::CHierarchicalMapMHPartition::saveGlobalMapForMATLAB
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:
Definition: CHierarchicalMapMHPartition.cpp:481
mrpt::hmtslam::CHierarchicalMapMHPartition::areNodesNeightbour
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.
Definition: CHierarchicalMapMHPartition.cpp:1059
mrpt::hmtslam::CHMHMapNode::TNodeID
mrpt::graphs::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:44
mrpt::hmtslam::CHierarchicalMapMHPartition::end
const_iterator end() const
Returns an iterator to the end of the list of nodes in the graph.
Definition: CHierarchicalMapMHPartition.h:55
mrpt::hmtslam::CHierarchicalMapMHPartition::findArcsBetweenNodes
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:
Definition: CHierarchicalMapMHPartition.cpp:1000
mrpt::hmtslam::CHierarchicalMapMHPartition::m_nodes
TNodeList m_nodes
The internal list of nodes and arcs in the whole hierarchical model.
Definition: CHierarchicalMapMHPartition.h:43
mrpt::hmtslam::TNodeList
std::map< CHMHMapNode::TNodeID, std::shared_ptr< CHMHMapNode > > TNodeList
A map between node IDs and nodes (used in HMT-SLAM).
Definition: CHMHMapNode.h:141
mrpt::hmtslam::CHierarchicalMapMHPartition::getAs3DScene
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.
Definition: CHierarchicalMapMHPartition.cpp:1094
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:29
mrpt::hmtslam::CHierarchicalMapMHPartition::getNodeByLabel
CHMHMapNode::Ptr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID)
Returns the node with the given label (case insensitive) for some given hypothesis ID,...
Definition: CHierarchicalMapMHPartition.cpp:78
mrpt::hmtslam::CHierarchicalMapMHPartition::m_arcs
TArcList m_arcs
Definition: CHierarchicalMapMHPartition.h:44
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::system::COutputLogger
Versatile class for consistent logging and management of output messages.
Definition: system/COutputLogger.h:117
CPose3DPDFSOG.h
mrpt::hmtslam::CHierarchicalMapMHPartition::arcCount
size_t arcCount() const
Returns the number of arcs in the partition:
Definition: CHierarchicalMapMHPartition.cpp:45
mrpt::hmtslam::CHierarchicalMapMHPartition::computeMatchProbabilityBetweenNodes
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...
Definition: CHierarchicalMapMHPartition.cpp:984
mrpt::hmtslam::TArcList
A class for storing a sequence of arcs (a path).
Definition: HMT_SLAM_common.h:103
mrpt::hmtslam::THypothesisID
int64_t THypothesisID
An integer number uniquely identifying each of the concurrent hypotheses for the robot topological pa...
Definition: HMT_SLAM_common.h:58
CHMHMapArc.h
mrpt::hmtslam::CHierarchicalMapMHPartition::saveAreasDiagramWithEllipsedForMATLAB
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...
Definition: CHierarchicalMapMHPartition.cpp:305
mrpt::hmtslam::CHMHMapArc::Ptr
std::shared_ptr< mrpt::hmtslam ::CHMHMapArc > Ptr
Definition: CHMHMapArc.h:35
mrpt::hmtslam::CHierarchicalMapMHPartition::const_iterator
TNodeList::const_iterator const_iterator
Definition: CHierarchicalMapMHPartition.h:48
mrpt::hmtslam::CHierarchicalMapMHPartition::computeCoordinatesTransformationBetweenNodes
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...
Definition: CHierarchicalMapMHPartition.cpp:855
mrpt::hmtslam::CHierarchicalMapMHPartition::computeGloballyConsistentNodeCoordinates
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...
Definition: CHierarchicalMapMHPartition.cpp:1295
mrpt::hmtslam::CHierarchicalMapMHPartition::getFirstNode
CHMHMapNode::Ptr getFirstNode()
Returns the first node in the graph, or nullptr if it does not exist.
Definition: CHierarchicalMapMHPartition.cpp:117
mrpt::hmtslam::CHierarchicalMapMHPartition::CHierarchicalMapMHPartition
CHierarchicalMapMHPartition()
Definition: CHierarchicalMapMHPartition.h:58
mrpt::hmtslam::CHierarchicalMapMHPartition
Represents a set of nodes and arcs, posibly only a part of the whole hierarchical,...
Definition: CHierarchicalMapMHPartition.h:36



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 15:15:43 UTC 2020