Main MRPT website > C++ reference
MRPT logo
CHierarchicalMapMHPartition.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CHierarchicalMapMHPartition_H
29 #define CHierarchicalMapMHPartition_H
30 
31 #include <mrpt/hmtslam/CLocalMetricHypothesis.h>
32 #include <mrpt/hmtslam/CHMHMapArc.h>
33 #include <mrpt/hmtslam/CHMHMapNode.h>
34 
35 #include <mrpt/poses/CPose3DPDFSOG.h>
36 #include <mrpt/poses/CPose3DPDFGaussian.h>
37 #include <mrpt/utils/CDebugOutputCapable.h>
38 
39 #include <map>
40 
41 namespace mrpt
42 {
43  namespace opengl
44  {
45  class COpenGLScene;
46  }
47  namespace poses
48  {
49  class CPose3DPDFParticles;
50  }
51 
52  namespace hmtslam
53  {
54  using namespace mrpt::opengl;
55  using namespace mrpt::slam;
56  using namespace mrpt::utils;
57 
58  /** Represents a set of nodes and arcs, posibly only a part of the whole hierarchical, multi-hypothesis map.
59  * A usar will never create an instance of this class, rather it will employ CHierarchicalMHMap.
60  * \sa CHierarchicalMHMap, CHMHMapArc, CHMHMapNode
61  * \ingroup mrpt_hmtslam_grp
62  */
63  class HMTSLAM_IMPEXP CHierarchicalMapMHPartition : public mrpt::utils::CDebugOutputCapable
64  {
65  protected:
66  /** The internal list of nodes and arcs in the whole hierarchical model.
67  * The objects must be deleted only in the CHierarchicalMap class, not in partitions only objects.
68  */
69  TNodeList m_nodes;
70  TArcList m_arcs;
71 
72  public:
73 
74  typedef TNodeList::iterator iterator;
75  typedef TNodeList::const_iterator const_iterator;
76 
77  /** Returns an iterator to the first node in the graph. */
78  const_iterator begin() const { return m_nodes.begin(); }
79 
80  /** Returns an iterator to the first node in the graph. */
81  iterator begin() { return m_nodes.begin(); }
82 
83  /** Returns an iterator to the end of the list of nodes in the graph. */
84  const_iterator end() const { return m_nodes.end(); }
85 
86  /** Returns an iterator to the end of the list of nodes in the graph. */
87  iterator end() { return m_nodes.end(); }
88 
89 
90  CHierarchicalMapMHPartition() : m_nodes(), m_arcs()
91  { }
92 
93  /** A type that reprensents a sequence of node IDs
94  */
95  typedef std::vector<CHMHMapNode::TNodeID> TNodeIDsList;
96 
97  /** Returns the number of nodes in the partition:
98  */
99  size_t nodeCount() const;
100 
101  /** Returns the number of arcs in the partition:
102  */
103  size_t arcCount() const;
104 
105  /** Returns the first node in the graph, or NULL if it does not exist.
106  * \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"
107  */
108  CHMHMapNodePtr getFirstNode();
109 
110  /** Returns the node with the given ID, or NULL if it does not exist.
111  * \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"
112  */
113  CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id);
114 
115  /** Returns the node with the given ID, or NULL if it does not exist.
116  * \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"
117  */
118  const CHMHMapNodePtr getNodeByID(CHMHMapNode::TNodeID id) const;
119 
120  /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist.
121  * \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"
122  */
123  CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID );
124 
125  /** Returns the node with the given label (case insensitive) for some given hypothesis ID, or NULL if it does not exist.
126  * \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"
127  */
128  const CHMHMapNodePtr getNodeByLabel(const std::string &label, const THypothesisID &hypothesisID) const;
129 
130  /** Returns a partition of this graph only with nodes at a given level in the hierarchy (0=ground level,1=parent level,etc)
131  * - The partition may be empty if no node fulfills the condition.
132  * - All arcs STARTING at each node from the partition will be added to the partition as well.
133  * - Levels in the hierarchy here stands for arcs of type "arcType_Belongs" only.
134  * \sa CHMHMapArc
135  */
136  //CHierarchicalMapMHPartition getPartitionByHiearchyLevel( unsigned int level );
137 
138  /** 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.
139  * ADDITIONAL NOTES:
140  * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
141  * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
142  */
143  void saveAreasDiagramForMATLAB(
144  const std::string &filName,
145  const CHMHMapNode::TNodeID &idReferenceNode,
146  const THypothesisID &hypothesisID) const;
147 
148  /** 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.
149  * ADDITIONAL NOTES:
150  * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
151  * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
152  */
153  void saveAreasDiagramWithEllipsedForMATLAB(
154  const std::string &filName,
155  const CHMHMapNode::TNodeID &idReferenceNode,
156  const THypothesisID &hypothesisID,
157  float uncertaintyExagerationFactor = 1.0f,
158  bool drawArcs = false,
159  unsigned int numberOfIterationsForOptimalGlobalPoses = 4
160  ) const;
161 
162  /** Saves a MATLAB script that represents graphically the reconstructed "global map"
163  * ADDITIONAL NOTES:
164  * - Coordinates are computed simply as the mean value of the first arc with an annotation "RelativePose", added to the pose of the original node.
165  * - If the coordinates of any node can not be computed (no arcs,...), an exception will be raised.
166  */
167  void saveGlobalMapForMATLAB(
168  const std::string &filName,
169  const THypothesisID &hypothesisID,
170  const CHMHMapNode::TNodeID &idReferenceNode ) const;
171 
172 
173  /** The Dijkstra algorithm for finding the shortest path between a pair of nodes.
174  * \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.
175  */
176  void findPathBetweenNodes(
177  const CHMHMapNode::TNodeID &nodeFrom,
178  const CHMHMapNode::TNodeID &nodeTo,
179  const THypothesisID &hypothesisID,
180  TArcList &out_path,
181  bool direction=false) const;
182 
183 
184  /** Draw a number of samples according to the PDF of the coordinates transformation between a pair of "Area"'s nodes.
185  * \exception std::exception If there is not enought information in arcs to compute the PDF
186  * \sa computeGloballyConsistentNodeCoordinates
187  */
188  void computeCoordinatesTransformationBetweenNodes(
189  const CHMHMapNode::TNodeID &nodeFrom,
190  const CHMHMapNode::TNodeID &nodeTo,
191  CPose3DPDFParticles &posePDF,
192  const THypothesisID &hypothesisID,
193  unsigned int particlesCount = 100,
194  float additionalNoiseXYratio = 0.02,
195  float additionalNoisePhiRad = DEG2RAD(0.1)
196  ) const;
197 
198  /** 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)
199  * If there is not enough information or a robust estimation cannot be found, there will not be particles in "estimatedRelativePose".
200  */
201  float computeMatchProbabilityBetweenNodes(
202  const CHMHMapNode::TNodeID &nodeFrom,
203  const CHMHMapNode::TNodeID &nodeTo,
204  float &maxMatchProb,
205  CPose3DPDFSOG &estimatedRelativePose,
206  const THypothesisID &hypothesisID,
207  unsigned int monteCarloSamplesPose = 300
208  );
209 
210  /** Returns all the arcs between a pair of nodes:
211  */
212  void findArcsBetweenNodes(
213  const CHMHMapNode::TNodeID &node1,
214  const CHMHMapNode::TNodeID &node2,
215  const THypothesisID &hypothesisID,
216  TArcList &out_listArcs ) const;
217 
218  /** Returns the arcs between a pair of nodes of a given type.
219  */
220  void findArcsOfTypeBetweenNodes(
221  const CHMHMapNode::TNodeID &node1id,
222  const CHMHMapNode::TNodeID &node2id,
223  const THypothesisID &hypothesisID,
224  const std::string &arcType,
225  TArcList &ret) const;
226 
227  /** Returns the first arc between a pair of nodes of a given type, and if it is in the opposite direction.
228  * \return The arc, or NULL if not found.
229  */
230  CHMHMapArcPtr findArcOfTypeBetweenNodes(
231  const CHMHMapNode::TNodeID &node1id,
232  const CHMHMapNode::TNodeID &node2id,
233  const THypothesisID &hypothesisID,
234  const std::string &arcType,
235  bool &isInverted ) const;
236 
237 
238  /** Returns wether two nodes are "neightbour", i.e. have a direct arc between them
239  */
240  bool areNodesNeightbour(
241  const CHMHMapNode::TNodeID &node1,
242  const CHMHMapNode::TNodeID &node2,
243  const THypothesisID &hypothesisID,
244  const char *requiredAnnotation=NULL ) const;
245 
246  /** 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.
247  * Global coordinates will be computed relative to the node "idReferenceNode".
248  * \exception std::exception If there is any node without a pose arc, invalid (non invertible) matrixes, etc...
249  * \sa computeCoordinatesTransformationBetweenNodes
250  */
251  void computeGloballyConsistentNodeCoordinates(
252  std::map<CHMHMapNode::TNodeID,CPose3DPDFGaussian, std::less<CHMHMapNode::TNodeID>, Eigen::aligned_allocator<std::pair<const CHMHMapNode::TNodeID,CPose3DPDFGaussian> > > &nodePoses,
253  const CHMHMapNode::TNodeID &idReferenceNode,
254  const THypothesisID &hypothesisID,
255  const unsigned int &numberOfIterations = 2) const;
256 
257  /** Returns a 3D scene reconstruction of the hierarchical map.
258  * See "computeGloballyConsistentNodeCoordinates" for the meaning of "numberOfIterationsForOptimalGlobalPoses"
259  */
260  void getAs3DScene(
261  COpenGLScene &outScene,
262  const CHMHMapNode::TNodeID &idReferenceNode,
263  const THypothesisID &hypothesisID,
264  const unsigned int &numberOfIterationsForOptimalGlobalPoses = 5,
265  const bool &showRobotPoseIDs = true
266  ) const;
267 
268  /** Return a textual description of the whole graph */
269  void dumpAsText(utils::CStringList &s) const;
270 
271 
272 
273  /** Computes the probability [0,1] of two areas' gridmaps to overlap, via a Monte Carlo aproximation.
274  * \exception std::exception If there is not enought information in arcs, etc...
275  * \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.
276  */
277  double computeOverlapProbabilityBetweenNodes(
278  const CHMHMapNode::TNodeID &nodeFrom,
279  const CHMHMapNode::TNodeID &nodeTo,
280  const THypothesisID &hypothesisID,
281  const size_t &monteCarloSamples = 100,
282  const float margin_to_substract = 6
283  ) const ;
284 
285  protected:
286 
287  }; // End of class def.
288  }
289 }
290 
291 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013