Main MRPT website > C++ reference
MRPT logo
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-2014, 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 
12 #include <mrpt/hmtslam/CLocalMetricHypothesis.h>
13 #include <mrpt/hmtslam/CHMHMapArc.h>
14 #include <mrpt/hmtslam/CHMHMapNode.h>
15 
16 #include <mrpt/poses/CPose3DPDFSOG.h>
17 #include <mrpt/poses/CPose3DPDFGaussian.h>
18 #include <mrpt/utils/CDebugOutputCapable.h>
19 
20 #include <map>
21 
22 namespace mrpt
23 {
24  namespace opengl
25  {
26  class COpenGLScene;
27  }
28  namespace poses
29  {
30  class CPose3DPDFParticles;
31  }
32 
33  namespace hmtslam
34  {
35  using namespace mrpt::opengl;
36  using namespace mrpt::slam;
37  using namespace mrpt::utils;
38 
39  /** Represents a set of nodes and arcs, posibly only a part of the whole hierarchical, multi-hypothesis map.
40  * A usar will never create an instance of this class, rather it will employ CHierarchicalMHMap.
41  * \sa CHierarchicalMHMap, CHMHMapArc, CHMHMapNode
42  * \ingroup mrpt_hmtslam_grp
43  */
44  class HMTSLAM_IMPEXP CHierarchicalMapMHPartition : public mrpt::utils::CDebugOutputCapable
45  {
46  protected:
47  /** The internal list of nodes and arcs in the whole hierarchical model.
48  * The objects must be deleted only in the CHierarchicalMap class, not in partitions only objects.
49  */
50  TNodeList m_nodes;
51  TArcList m_arcs;
52 
53  public:
54 
55  typedef TNodeList::iterator iterator;
56  typedef TNodeList::const_iterator const_iterator;
57 
58  /** Returns an iterator to the first node in the graph. */
59  const_iterator begin() const { return m_nodes.begin(); }
60 
61  /** Returns an iterator to the first node in the graph. */
62  iterator begin() { return m_nodes.begin(); }
63 
64  /** Returns an iterator to the end of the list of nodes in the graph. */
65  const_iterator end() const { return m_nodes.end(); }
66 
67  /** Returns an iterator to the end of the list of nodes in the graph. */
68  iterator end() { return m_nodes.end(); }
69 
70 
71  CHierarchicalMapMHPartition() : m_nodes(), m_arcs()
72  { }
73 
74  /** A type that reprensents a sequence of node IDs
75  */
76  typedef std::vector<CHMHMapNode::TNodeID> TNodeIDsList;
77 
78  /** Returns the number of nodes in the partition:
79  */
80  size_t nodeCount() const;
81 
82  /** Returns the number of arcs in the partition:
83  */
84  size_t arcCount() const;
85 
86  /** Returns the first node in the graph, 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  CHMHMapNodePtr getFirstNode();
90 
91  /** Returns the node with the given 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 getNodeByID(CHMHMapNode::TNodeID id);
95 
96  /** Returns the node with the given 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 getNodeByID(CHMHMapNode::TNodeID id) const;
100 
101  /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist.
102  * \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"
103  */
104  CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID );
105 
106  /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist.
107  * \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"
108  */
109  const CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID) const;
110 
111  /** Returns a partition of this graph only with nodes at a given level in 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 the partition as well.
114  * - Levels in the hierarchy here stands for arcs of type "arcType_Belongs" only.
115  * \sa CHMHMapArc
116  */
117  //CHierarchicalMapMHPartition getPartitionByHiearchyLevel( unsigned int level );
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.
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 saveAreasDiagramForMATLAB(
125  const std::string &filName,
126  const CHMHMapNode::TNodeID &idReferenceNode,
127  const THypothesisID &hypothesisID) const;
128 
129  /** 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.
130  * ADDITIONAL NOTES:
131  * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
132  * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
133  */
134  void saveAreasDiagramWithEllipsedForMATLAB(
135  const std::string &filName,
136  const CHMHMapNode::TNodeID &idReferenceNode,
137  const THypothesisID &hypothesisID,
138  float uncertaintyExagerationFactor = 1.0f,
139  bool drawArcs = false,
140  unsigned int numberOfIterationsForOptimalGlobalPoses = 4
141  ) const;
142 
143  /** Saves a MATLAB script that represents graphically the reconstructed "global map"
144  * ADDITIONAL NOTES:
145  * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
146  * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
147  */
148  void saveGlobalMapForMATLAB(
149  const std::string &filName,
150  const THypothesisID &hypothesisID,
151  const CHMHMapNode::TNodeID &idReferenceNode ) const;
152 
153 
154  /** The Dijkstra algorithm for finding the shortest path between a pair of nodes.
155  * \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.
156  */
157  void findPathBetweenNodes(
158  const CHMHMapNode::TNodeID &nodeFrom,
159  const CHMHMapNode::TNodeID &nodeTo,
160  const THypothesisID &hypothesisID,
161  TArcList &out_path,
162  bool direction=false) const;
163 
164 
165  /** Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Area"'s nodes.
166  * \exception std::exception If there is not enought information in arcs to compute the PDF
167  * \sa computeGloballyConsistentNodeCoordinates
168  */
169  void computeCoordinatesTransformationBetweenNodes(
170  const CHMHMapNode::TNodeID &nodeFrom,
171  const CHMHMapNode::TNodeID &nodeTo,
172  CPose3DPDFParticles &posePDF,
173  const THypothesisID &hypothesisID,
174  unsigned int particlesCount = 100,
175  float additionalNoiseXYratio = 0.02,
176  float additionalNoisePhiRad = DEG2RAD(0.1)
177  ) const;
178 
179  /** 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)
180  * If there is not enough information or a robust estimation cannot be found, there will not be particles in "estimatedRelativePose".
181  */
182  float computeMatchProbabilityBetweenNodes(
183  const CHMHMapNode::TNodeID &nodeFrom,
184  const CHMHMapNode::TNodeID &nodeTo,
185  float &maxMatchProb,
186  CPose3DPDFSOG &estimatedRelativePose,
187  const THypothesisID &hypothesisID,
188  unsigned int monteCarloSamplesPose = 300
189  );
190 
191  /** Returns all the arcs between a pair of nodes:
192  */
193  void findArcsBetweenNodes(
194  const CHMHMapNode::TNodeID &node1,
195  const CHMHMapNode::TNodeID &node2,
196  const THypothesisID &hypothesisID,
197  TArcList &out_listArcs ) const;
198 
199  /** Returns the arcs between a pair of nodes of a given type.
200  */
201  void findArcsOfTypeBetweenNodes(
202  const CHMHMapNode::TNodeID &node1id,
203  const CHMHMapNode::TNodeID &node2id,
204  const THypothesisID &hypothesisID,
205  const std::string &arcType,
206  TArcList &ret) const;
207 
208  /** Returns the first arc between a pair of nodes of a given type, and if it is in the opposite direction.
209  * \return The arc, or NULL if not found.
210  */
211  CHMHMapArcPtr findArcOfTypeBetweenNodes(
212  const CHMHMapNode::TNodeID &node1id,
213  const CHMHMapNode::TNodeID &node2id,
214  const THypothesisID &hypothesisID,
215  const std::string &arcType,
216  bool &isInverted ) const;
217 
218 
219  /** Returns wether two nodes are "neightbour", i.e. have a direct arc between them
220  */
221  bool areNodesNeightbour(
222  const CHMHMapNode::TNodeID &node1,
223  const CHMHMapNode::TNodeID &node2,
224  const THypothesisID &hypothesisID,
225  const char *requiredAnnotation=NULL ) const;
226 
227  /** 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.
228  * Global coordinates will be computed relative to the node "idReferenceNode".
229  * \exception std::exception If there is any node without a pose arc, invalid (non invertible) matrixes, etc...
230  * \sa computeCoordinatesTransformationBetweenNodes
231  */
232  void computeGloballyConsistentNodeCoordinates(
233  std::map<CHMHMapNode::TNodeID,CPose3DPDFGaussian, std::less<CHMHMapNode::TNodeID>, Eigen::aligned_allocator<std::pair<const CHMHMapNode::TNodeID,CPose3DPDFGaussian> > > &nodePoses,
234  const CHMHMapNode::TNodeID &idReferenceNode,
235  const THypothesisID &hypothesisID,
236  const unsigned int &numberOfIterations = 2) const;
237 
238  /** Returns a 3D scene reconstruction of the hierarchical map.
239  * See "computeGloballyConsistentNodeCoordinates" for the meaning of "numberOfIterationsForOptimalGlobalPoses"
240  */
241  void getAs3DScene(
242  COpenGLScene &outScene,
243  const CHMHMapNode::TNodeID &idReferenceNode,
244  const THypothesisID &hypothesisID,
245  const unsigned int &numberOfIterationsForOptimalGlobalPoses = 5,
246  const bool &showRobotPoseIDs = true
247  ) const;
248 
249  /** Return a textual description of the whole graph */
250  void dumpAsText(utils::CStringList &s) const;
251 
252 
253 
254  /** Computes the probability [0,1] of two areas' gridmaps to overlap, via a Monte Carlo aproximation.
255  * \exception std::exception If there is not enought information in arcs, etc...
256  * \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.
257  */
258  double computeOverlapProbabilityBetweenNodes(
259  const CHMHMapNode::TNodeID &nodeFrom,
260  const CHMHMapNode::TNodeID &nodeTo,
261  const THypothesisID &hypothesisID,
262  const size_t &monteCarloSamples = 100,
263  const float margin_to_substract = 6
264  ) const ;
265 
266  protected:
267 
268  }; // End of class def.
269  }
270 }
271 
272 #endif
mrpt::hmtslam::CHierarchicalMapMHPartition::TNodeIDsList
std::vector< CHMHMapNode::TNodeID > TNodeIDsList
A type that reprensents a sequence of node IDs.
Definition: CHierarchicalMapMHPartition.h:76
HMTSLAM_IMPEXP
#define HMTSLAM_IMPEXP
Definition: hmtslam_impexp.h:81
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::hmtslam::CHierarchicalMapMHPartition
Represents a set of nodes and arcs, posibly only a part of the whole hierarchical, multi-hypothesis map.
Definition: CHierarchicalMapMHPartition.h:44
CPose3DPDFSOG.h
CHMHMapNode.h
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::poses::CPose3DPDFSOG
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:34
mrpt::hmtslam::TNodeList
std::map< CHMHMapNode::TNodeID, CHMHMapNodePtr > TNodeList
A map between node IDs and nodes (used in HMT-SLAM).
Definition: CHMHMapNode.h:150
CDebugOutputCapable.h
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::slam
This namespace contains algorithms for SLAM, localization, map building, representation of robot's ac...
Definition: CParticleFilter.h:18
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:62
mrpt::hmtslam::CHierarchicalMapMHPartition::begin
const_iterator begin() const
Returns an iterator to the first node in the graph.
Definition: CHierarchicalMapMHPartition.h:59
Eigen::aligned_allocator
Definition: aligned_containers.h:19
DEG2RAD
#define DEG2RAD(x)
Definition: Utils.h:61
mrpt::utils::CStringList
A class for storing a list of text lines.
Definition: CStringList.h:32
mrpt::hmtslam::CHierarchicalMapMHPartition::begin
iterator begin()
Returns an iterator to the first node in the graph.
Definition: CHierarchicalMapMHPartition.h:62
CHMHMapArc.h
mrpt::hmtslam::CHierarchicalMapMHPartition::end
iterator end()
Returns an iterator to the end of the list of nodes in the graph.
Definition: CHierarchicalMapMHPartition.h:68
CPose3DPDFGaussian.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CHierarchicalMapMHPartition.h:24
mrpt::opengl::COpenGLScene
This class allows the user to create, load, save, and render 3D scenes using OpenGL primitives...
Definition: COpenGLScene.h:49
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:37
CLocalMetricHypothesis.h
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:65
mrpt::hmtslam::CHierarchicalMapMHPartition::m_nodes
TNodeList m_nodes
The internal list of nodes and arcs in the whole hierarchical model.
Definition: CHierarchicalMapMHPartition.h:50
mrpt::hmtslam::TArcList
A class for storing a sequence of arcs (a path).
Definition: HMT_SLAM_common.h:121
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::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:33
mrpt::hmtslam::CHMHMapNode::TNodeID
mrpt::utils::TNodeID TNodeID
The type of the IDs of nodes.
Definition: CHMHMapNode.h:51


Page generated by Doxygen 1.8.8 for MRPT 1.2.2 SVN:Unversioned directory at Tue Oct 14 02:14:08 UTC 2014