Main MRPT website > C++ reference for MRPT 1.3.2
maps/CHeightGridMap2D.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-2015, 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 
10 #ifndef CHeightGridMap2D_H
11 #define CHeightGridMap2D_H
12 
16 #include <mrpt/utils/color_maps.h>
17 #include <mrpt/utils/TEnumType.h>
18 #include <mrpt/maps/CMetricMap.h>
19 #include <mrpt/maps/link_pragmas.h>
20 #include <mrpt/poses/poses_frwds.h>
21 #include <mrpt/obs/obs_frwds.h>
22 
23 namespace mrpt
24 {
25  namespace maps
26  {
27  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CHeightGridMap2D, CMetricMap, MAPS_IMPEXP )
28 
29  /** The contents of each cell in a CHeightGridMap2D map.
30  **/
32  {
33  /** Constructor
34  */
35  THeightGridmapCell() : h(0), w(0)
36  {}
37 
38  /** The current average height (in meters).
39  */
40  float h;
41 
42  /** The current standard deviation of the height (in meters).
43  */
44  float var;
45 
46  /** Auxiliary variable for storing the incremental mean value (in meters).
47  */
48  float u;
49 
50  /** Auxiliary (in meters).
51  */
52  float v;
53 
54 
55  /** [mrSimpleAverage model] The accumulated weight: initially zero if un-observed, increased by one for each observation.
56  */
57  uint32_t w;
58  };
59 
60  /** A mesh representation of a surface which keeps the estimated height for each (x,y) location.
61  * Important implemented features are the insertion of 2D laser scans (from arbitrary 6D poses) and the exportation as 3D scenes.
62  *
63  * Each cell contains the up-to-date average height from measured falling in that cell. Algorithms that can be used:
64  * - mrSimpleAverage: Each cell only stores the current average value.
65  */
66  class MAPS_IMPEXP CHeightGridMap2D : public mrpt::maps::CMetricMap, public utils::CDynamicGrid<THeightGridmapCell>
67  {
68  // This must be added to any CSerializable derived class:
70  public:
71 
72  /** Calls the base CMetricMap::clear
73  * Declared here to avoid ambiguity between the two clear() in both base classes.
74  */
75  inline void clear() { CMetricMap::clear(); }
76 
77  float cell2float(const THeightGridmapCell& c) const
78  {
79  return float(c.h);
80  }
81 
82  /** The type of map representation to be used.
83  * See mrpt::maps::CHeightGridMap2D for discussion.
84  */
86  {
87  mrSimpleAverage = 0
88 // mrSlidingWindow
89  };
90 
91  /** Constructor
92  */
94  TMapRepresentation mapType = mrSimpleAverage,
95  float x_min = -2,
96  float x_max = 2,
97  float y_min = -2,
98  float y_max = 2,
99  float resolution = 0.1
100  );
101 
102  /** Returns true if the map is empty/no observation has been inserted.
103  */
104  bool isEmpty() const;
105 
106  /** Parameters related with inserting observations into the map.
107  */
109  {
110  /** Default values loader:
111  */
113 
114  /** See utils::CLoadableOptions
115  */
116  void loadFromConfigFile(
117  const mrpt::utils::CConfigFileBase &source,
118  const std::string &section);
119 
120  /** See utils::CLoadableOptions
121  */
122  void dumpToTextStream(mrpt::utils::CStream &out) const;
123 
124  /** Wether to perform filtering by z-coordinate (default=false): coordinates are always RELATIVE to the robot for this filter.
125  */
127 
128  /** Only when filterByHeight is true: coordinates are always RELATIVE to the robot for this filter.
129  */
130  float z_min,z_max;
131 
132  float minDistBetweenPointsWhenInserting; //!< When inserting a scan, a point cloud is first created with this minimum distance between the 3D points (default=0).
133 
135 
136  } insertionOptions;
137 
138  /** See docs in base class: in this class it always returns 0 */
139  float compute3DMatchingRatio(
140  const mrpt::maps::CMetricMap *otherMap,
141  const mrpt::poses::CPose3D &otherMapPose,
142  float maxDistForCorr = 0.10f,
143  float maxMahaDistForCorr = 2.0f
144  ) const;
145 
146  /** The implementation in this class just calls all the corresponding method of the contained metric maps.
147  */
148  void saveMetricMapRepresentationToFile(
149  const std::string &filNamePrefix
150  ) const;
151 
152  /** Returns a 3D object representing the map: by default, it will be a mrpt::opengl::CMesh object, unless
153  * it is specified otherwise in mrpt::
154  */
155  void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
156 
157  /** Return the type of the gas distribution map, according to parameters passed on construction.
158  */
159  TMapRepresentation getMapType();
160 
161 
162  /** Gets the intersection between a 3D line and a Height Grid map (taking into account the different heights of each individual cell).
163  */
164  bool intersectLine3D(const mrpt::math::TLine3D &r1, mrpt::math::TObject3D &obj) const;
165 
166  /** Computes the minimum and maximum height in the grid.
167  * \return False if there is no observed cell yet.
168  */
169  bool getMinMaxHeight(float &z_min, float &z_max) const;
170 
171  /** Return the number of cells with at least one height data inserted. */
172  size_t countObservedCells() const;
173 
174  protected:
175  TMapRepresentation m_mapType; //!< The map representation type of this map
176 
177  // See docs in base class
178  void internal_clear() MRPT_OVERRIDE;
179  bool internal_insertObservation( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
180  double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) MRPT_OVERRIDE;
181 
183  float min_x,max_x,min_y,max_y,resolution; //!< See CHeightGridMap2D::CHeightGridMap2D
184  mrpt::maps::CHeightGridMap2D::TMapRepresentation mapType; //!< The kind of map representation (see CHeightGridMap2D::CHeightGridMap2D)
185  mrpt::maps::CHeightGridMap2D::TInsertionOptions insertionOpts;
186  MAP_DEFINITION_END(CHeightGridMap2D,MAPS_IMPEXP)
187  };
188  DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE( CHeightGridMap2D, CMetricMap, MAPS_IMPEXP )
189 
190  } // End of namespace
191 
192  namespace global_settings
193  {
194  /** If set to true (default), mrpt::maps::CHeightGridMap2D will be exported as a opengl::CMesh, otherwise, as a opengl::CPointCloudColoured
195  * Affects to:
196  * - CHeightGridMap2D::getAs3DObject
197  */
198  extern MAPS_IMPEXP bool HEIGHTGRIDMAP_EXPORT3D_AS_MESH;
199  }
200 
201  // Specializations MUST occur at the same namespace:
202  namespace utils
203  {
204  template <>
206  {
208  static void fill(bimap<enum_t,std::string> &m_map)
209  {
210  m_map.insert(maps::CHeightGridMap2D::mrSimpleAverage, "mrSimpleAverage");
211  }
212  };
213  } // End of namespace
214 
215 } // End of namespace
216 
217 #endif
A mesh representation of a surface which keeps the estimated height for each (x,y) location...
float var
The current standard deviation of the height (in meters).
float h
The current average height (in meters).
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
TColormap
Different colormaps.
Definition: color_maps.h:48
TMapRepresentation
The type of map representation to be used.
void clear()
Erase all the contents of the map.
The contents of each cell in a CHeightGridMap2D map.
Standard object for storing any 3D lightweight object.
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
uint32_t w
[mrSimpleAverage model] The accumulated weight: initially zero if un-observed, increased by one for e...
float v
Auxiliary (in meters).
This class allows loading and storing values and vectors of different types from a configuration text...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
float z_min
Only when filterByHeight is true: coordinates are always RELATIVE to the robot for this filter...
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:39
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
Parameters related with inserting observations into the map.
A bidirectional version of std::map, declared as bimap and which actually contains two std...
Definition: bimap.h:28
float minDistBetweenPointsWhenInserting
When inserting a scan, a point cloud is first created with this minimum distance between the 3D point...
float u
Auxiliary variable for storing the incremental mean value (in meters).
float cell2float(const THeightGridmapCell &c) const
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Declares a virtual base class for all metric maps storage classes.
bool filterByHeight
Wether to perform filtering by z-coordinate (default=false): coordinates are always RELATIVE to the r...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
TMapRepresentation m_mapType
The map representation type of this map.
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
MAPS_IMPEXP bool HEIGHTGRIDMAP_EXPORT3D_AS_MESH
If set to true (default), mrpt::maps::CHeightGridMap2D will be exported as a opengl::CMesh, otherwise, as a opengl::CPointCloudColoured Affects to:
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
3D line, represented by a base point and a director vector.



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Thu Dec 10 00:07:55 UTC 2015