Main MRPT website > C++ reference
MRPT logo
CHeightGridMap2D.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 
29 #ifndef CHeightGridMap2D_H
30 #define CHeightGridMap2D_H
31 
32 #include <mrpt/utils/CDynamicGrid.h>
33 #include <mrpt/utils/CSerializable.h>
34 #include <mrpt/math/CMatrixD.h>
35 #include <mrpt/math/geometry.h>
36 #include <mrpt/system/os.h>
37 #include <mrpt/utils/CLoadableOptions.h>
38 #include <mrpt/utils/stl_extensions.h>
39 
40 #include <mrpt/slam/CMetricMap.h>
41 
42 #include <mrpt/maps/link_pragmas.h>
43 
44 namespace mrpt
45 {
46  namespace poses
47  {
48  class CPose2D;
49  }
50  namespace slam
51  {
52  using namespace mrpt::utils;
53 
54  class CObservation;
55 
56  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CHeightGridMap2D, CMetricMap, MAPS_IMPEXP )
57 
58  /** The contents of each cell in a CHeightGridMap2D map.
59  **/
60  struct MAPS_IMPEXP THeightGridmapCell
61  {
62  /** Constructor
63  */
64  THeightGridmapCell() : h(0), w(0)
65  {}
66 
67  /** The current average height (in meters).
68  */
69  float h;
70 
71  /** The current standard deviation of the height (in meters).
72  */
73  float var;
74 
75  /** Auxiliary variable for storing the incremental mean value (in meters).
76  */
77  float u;
78 
79  /** Auxiliary (in meters).
80  */
81  float v;
82 
83 
84  /** [mrSimpleAverage model] The accumulated weight: initially zero if un-observed, increased by one for each observation.
85  */
86  uint32_t w;
87  };
88 
89  /** A mesh representation of a surface which keeps the estimated height for each (x,y) location.
90  * Important implemented features are the insertion of 2D laser scans (from arbitrary 6D poses) and the exportation as 3D scenes.
91  *
92  * Each cell contains the up-to-date average height from measured falling in that cell. Algorithms that can be used:
93  * - mrSimpleAverage: Each cell only stores the current average value.
94  */
95  class MAPS_IMPEXP CHeightGridMap2D : public CMetricMap, public utils::CDynamicGrid<THeightGridmapCell>
96  {
97  // This must be added to any CSerializable derived class:
98  DEFINE_SERIALIZABLE( CHeightGridMap2D )
99  public:
100 
101  /** Calls the base CMetricMap::clear
102  * Declared here to avoid ambiguity between the two clear() in both base classes.
103  */
104  inline void clear() { CMetricMap::clear(); }
105 
106  float cell2float(const THeightGridmapCell& c) const
107  {
108  return float(c.h);
109  }
110 
111  /** The type of map representation to be used.
112  * See mrpt::slam::CHeightGridMap2D for discussion.
113  */
114  enum TMapRepresentation
115  {
116  mrSimpleAverage = 0
117 // mrSlidingWindow
118  };
119 
120  /** Constructor
121  */
122  CHeightGridMap2D(
123  TMapRepresentation mapType = mrSimpleAverage,
124  float x_min = -2,
125  float x_max = 2,
126  float y_min = -2,
127  float y_max = 2,
128  float resolution = 0.1
129  );
130 
131  /** Returns true if the map is empty/no observation has been inserted.
132  */
133  bool isEmpty() const;
134 
135  /** Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map.
136  *
137  * \param takenFrom The robot's pose the observation is supposed to be taken from.
138  * \param obs The observation.
139  * \return This method returns a likelihood in the range [0,1].
140  *
141  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
142  */
143  double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
144 
145  /** Parameters related with inserting observations into the map.
146  */
147  struct MAPS_IMPEXP TInsertionOptions : public utils::CLoadableOptions
148  {
149  /** Default values loader:
150  */
151  TInsertionOptions();
152 
153  /** See utils::CLoadableOptions
154  */
155  void loadFromConfigFile(
156  const mrpt::utils::CConfigFileBase &source,
157  const std::string &section);
158 
159  /** See utils::CLoadableOptions
160  */
161  void dumpToTextStream(CStream &out) const;
162 
163  /** Wether to perform filtering by z-coordinate (default=false): coordinates are always RELATIVE to the robot for this filter.
164  */
165  bool filterByHeight;
166 
167  /** Only when filterByHeight is true: coordinates are always RELATIVE to the robot for this filter.
168  */
169  float z_min,z_max;
170 
171  float minDistBetweenPointsWhenInserting; //!< When inserting a scan, a point cloud is first created with this minimum distance between the 3D points (default=0).
172 
173  } insertionOptions;
174 
175  /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
176  * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
177  * \param otherMap [IN] The other map to compute the matching with.
178  * \param otherMapPose [IN] The 6D pose of the other map as seen from "this".
179  * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
180  * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
181  *
182  * \return The matching ratio [0,1]
183  * \sa computeMatchingWith2D
184  */
185  float compute3DMatchingRatio(
186  const CMetricMap *otherMap,
187  const CPose3D &otherMapPose,
188  float minDistForCorr = 0.10f,
189  float minMahaDistForCorr = 2.0f
190  ) const;
191 
192  /** The implementation in this class just calls all the corresponding method of the contained metric maps.
193  */
194  void saveMetricMapRepresentationToFile(
195  const std::string &filNamePrefix
196  ) const;
197 
198  /** Returns a 3D object representing the map: by default, it will be a mrpt::opengl::CMesh object, unless
199  * it is specified otherwise in mrpt::
200  */
201  void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
202 
203  /** Return the type of the gas distribution map, according to parameters passed on construction.
204  */
205  TMapRepresentation getMapType();
206 
207 
208  /** Gets the intersection between a 3D line and a Height Grid map (taking into account the different heights of each individual cell).
209  */
210  bool intersectLine3D(const TLine3D &r1, TObject3D &obj) const;
211 
212  /** Computes the minimum and maximum height in the grid.
213  * \return False if there is no observed cell yet.
214  */
215  bool getMinMaxHeight(float &z_min, float &z_max) const;
216 
217  /** Return the number of cells with at least one height data inserted. */
218  size_t countObservedCells() const;
219 
220  protected:
221 
222  /** The map representation type of this map.
223  */
224  TMapRepresentation m_mapType;
225 
226  /** Erase all the contents of the map
227  */
228  virtual void internal_clear();
229 
230  /** Insert the observation information into this map. This method must be implemented
231  * in derived classes.
232  * \param obs The observation
233  * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg)
234  *
235  * \sa CObservation::insertObservationInto
236  */
237  virtual bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
238 
239  };
240 
241 
242  } // End of namespace
243 
244  namespace global_settings
245  {
246  /** If set to true (default), mrpt::slam::CHeightGridMap2D will be exported as a opengl::CMesh, otherwise, as a opengl::CPointCloudColoured
247  * Affects to:
248  * - CHeightGridMap2D::getAs3DObject
249  */
250  extern MAPS_IMPEXP bool HEIGHTGRIDMAP_EXPORT3D_AS_MESH;
251  }
252 
253  // Specializations MUST occur at the same namespace:
254  namespace utils
255  {
256  template <>
257  struct TEnumTypeFiller<slam::CHeightGridMap2D::TMapRepresentation>
258  {
259  typedef slam::CHeightGridMap2D::TMapRepresentation enum_t;
260  static void fill(bimap<enum_t,std::string> &m_map)
261  {
262  m_map.insert(slam::CHeightGridMap2D::mrSimpleAverage, "mrSimpleAverage");
263  }
264  };
265  } // End of namespace
266 
267 } // End of namespace
268 
269 #endif


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