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 
23 namespace 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 
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  */
79  CHMHMapNodePtr getFirstNode();
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  */
84  CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id);
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  */
89  const CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id) const;
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  */
114  void saveAreasDiagramForMATLAB(
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  */
124  void saveAreasDiagramWithEllipsedForMATLAB(
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  */
138  void saveGlobalMapForMATLAB(
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  */
147  void findPathBetweenNodes(
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  */
159  void computeCoordinatesTransformationBetweenNodes(
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  */
172  float computeMatchProbabilityBetweenNodes(
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  */
183  void findArcsBetweenNodes(
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  */
191  void findArcsOfTypeBetweenNodes(
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  */
201  CHMHMapArcPtr findArcOfTypeBetweenNodes(
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 */
210  bool areNodesNeightbour(
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  */
221  void computeGloballyConsistentNodeCoordinates(
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  */
230  void getAs3DScene(
231  mrpt::opengl::COpenGLScene &outScene,
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 */
239  void dumpAsText(utils::CStringList &s) const;
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  */
247  double computeOverlapProbabilityBetweenNodes(
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::poses::CPose3DPDFSOG
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose .
Definition: CPose3DPDFSOG.h:34
mrpt::hmtslam::CHierarchicalMapMHPartition::end
iterator end()
Returns an iterator to the end of the list of nodes in the graph.
Definition: CHierarchicalMapMHPartition.h:58
mrpt::hmtslam::CHierarchicalMapMHPartition::begin
iterator begin()
Returns an iterator to the first node in the graph.
Definition: CHierarchicalMapMHPartition.h:52
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::utils::CDebugOutputCapable
This base class provides a common printf-like method to send debug information to std::cout,...
Definition: CDebugOutputCapable.h:31
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:37
mrpt::hmtslam::CHierarchicalMapMHPartition::iterator
TNodeList::iterator iterator
Definition: CHierarchicalMapMHPartition.h:45
CDebugOutputCapable.h
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:49
CPose3DPDFGaussian.h
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:60
mrpt::hmtslam::CHierarchicalMapMHPartition::begin
const_iterator begin() const
Returns an iterator to the first node in the graph.
Definition: CHierarchicalMapMHPartition.h:49
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
const_iterator
const typedef Scalar * const_iterator
Definition: eigen_plugins.h:24
CLocalMetricHypothesis.h
mrpt::utils::DEG2RAD
double DEG2RAD(const double x)
Degrees to radians.
Definition: bits.h:69
mrpt::hmtslam::CHierarchicalMapMHPartition::const_iterator
TNodeList::const_iterator const_iterator
Definition: CHierarchicalMapMHPartition.h:46
mrpt::hmtslam::CHierarchicalMapMHPartition::TNodeIDsList
std::vector< CHMHMapNode::TNodeID > TNodeIDsList
A type that reprensents a sequence of node IDs.
Definition: CHierarchicalMapMHPartition.h:66
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::m_nodes
TNodeList m_nodes
The internal list of nodes and arcs in the whole hierarchical model.
Definition: CHierarchicalMapMHPartition.h:40
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:33
mrpt::hmtslam::CHierarchicalMapMHPartition::m_arcs
TArcList m_arcs
Definition: CHierarchicalMapMHPartition.h:41
Eigen::aligned_allocator
Definition: aligned_containers.h:19
CPose3DPDFSOG.h
HMTSLAM_IMPEXP
#define HMTSLAM_IMPEXP
Definition: hmtslam_impexp.h:81
mrpt::hmtslam::TArcList
A class for storing a sequence of arcs (a path).
Definition: HMT_SLAM_common.h:119
mrpt::hmtslam::TNodeList
std::map< CHMHMapNode::TNodeID, CHMHMapNodePtr > TNodeList
A map between node IDs and nodes (used in HMT-SLAM).
Definition: CHMHMapNode.h:148
CHMHMapArc.h
mrpt::hmtslam::CHMHMapNode::TNodeID
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:49
mrpt::utils::CStringList
A class for storing a list of text lines.
Definition: CStringList.h:32
mrpt::hmtslam::CHierarchicalMapMHPartition::CHierarchicalMapMHPartition
CHierarchicalMapMHPartition()
Definition: CHierarchicalMapMHPartition.h:61
mrpt::hmtslam::CHierarchicalMapMHPartition
Represents a set of nodes and arcs, posibly only a part of the whole hierarchical,...
Definition: CHierarchicalMapMHPartition.h:34



Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 22:32:58 UTC 2019