Main MRPT website > C++ reference for MRPT 1.4.0
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-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
10#ifndef CHeightGridMap2D_H
11#define CHeightGridMap2D_H
12
23#include <mrpt/obs/obs_frwds.h>
24
25namespace mrpt
26{
27 namespace maps
28 {
30
31 /** The contents of each cell in a CHeightGridMap2D map */
33 {
34 float h; //!< The current average height (in meters)
35 float var; //!< The current standard deviation of the height (in meters)
36 float u; //!< Auxiliary variable for storing the incremental mean value (in meters).
37 float v; //!< Auxiliary (in meters)
38 uint32_t w; //!< [For mrSimpleAverage model] The accumulated weight: initially zero if un-observed, increased by one for each observation
39
40 THeightGridmapCell() : h(),var(),u(),v(),w() {}
41 };
42
43 /** Digital Elevation Model (DEM), a mesh or grid representation of a surface which keeps the estimated height for each (x,y) location.
44 * Important implemented features are the insertion of 2D laser scans (from arbitrary 6D poses) and the exportation as 3D scenes.
45 *
46 * Each cell contains the up-to-date average height from measured falling in that cell. Algorithms that can be used:
47 * - mrSimpleAverage: Each cell only stores the current average value.
48 *
49 * This class implements generic version of mrpt::maps::CMetric::insertObservation() accepting these types of sensory data:
50 * - mrpt::obs::CObservation2DRangeScan: 2D range scans
51 * - mrpt::obs::CObservationVelodyneScan
52 *
53 * \ingroup mrpt_maps_grp
54 */
57 public utils::CDynamicGrid<THeightGridmapCell>,
59 {
60 // This must be added to any CSerializable derived class:
62 public:
63
64 /** Calls the base CMetricMap::clear
65 * Declared here to avoid ambiguity between the two clear() in both base classes.
66 */
67 inline void clear() { CMetricMap::clear(); }
68
70 return float(c.h);
71 }
72
73 /** The type of map representation to be used.
74 * See mrpt::maps::CHeightGridMap2D for discussion.
75 */
77 {
78 mrSimpleAverage = 0
79 };
80
81 /** Constructor */
83 TMapRepresentation mapType = mrSimpleAverage,
84 double x_min = -2, double x_max = 2,
85 double y_min = -2, double y_max = 2,
86 double resolution = 0.1
87 );
88
89 bool isEmpty() const MRPT_OVERRIDE; //!< Returns true if the map is empty/no observation has been inserted.
90
91 /** Parameters related with inserting observations into the map */
92 struct MAPS_IMPEXP TInsertionOptions : public utils::CLoadableOptions
93 {
94 TInsertionOptions(); //!< Default values loader
95
96 void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
97 void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
98
99 bool filterByHeight; //!< Whether to perform filtering by z-coordinate (default=false): coordinates are always RELATIVE to the robot for this filter.vvv
100 float z_min,z_max; //!< Only when filterByHeight is true: coordinates are always RELATIVE to the robot for this filter.
101
103 } insertionOptions;
104
105 /** See docs in base class: in this class it always returns 0 */
106 float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
107
108 void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE; // See base class docs
109
110 /** Returns a 3D object representing the map: by default, it will be a mrpt::opengl::CMesh object, unless
111 * it is specified otherwise in mrpt::global_settings::HEIGHTGRIDMAP_EXPORT3D_AS_MESH */
113
114 /** Return the type of the gas distribution map, according to parameters passed on construction */
116
117 /** Return the number of cells with at least one height data inserted. */
118 size_t countObservedCells() const;
119
120 virtual bool insertIndividualPoint(const double x,const double y,const double z, const CHeightGridMap2D_Base::TPointInsertParams & params = CHeightGridMap2D_Base::TPointInsertParams() ) MRPT_OVERRIDE;
121 virtual double dem_get_resolution() const MRPT_OVERRIDE;
122 virtual size_t dem_get_size_x() const MRPT_OVERRIDE;
123 virtual size_t dem_get_size_y() const MRPT_OVERRIDE;
124 virtual bool dem_get_z_by_cell(const size_t cx, const size_t cy, double &z_out) const MRPT_OVERRIDE;
125 virtual bool dem_get_z(const double x, const double y, double &z_out) const MRPT_OVERRIDE;
126 virtual void dem_update_map() MRPT_OVERRIDE;
127
128 TMapRepresentation m_mapType; //!< The map representation type of this map
129
130 // See docs in base class
131 void internal_clear() MRPT_OVERRIDE;
132 bool internal_insertObservation( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
133 double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) MRPT_OVERRIDE;
134
136 double min_x,max_x,min_y,max_y,resolution; //!< See CHeightGridMap2D::CHeightGridMap2D
137 mrpt::maps::CHeightGridMap2D::TMapRepresentation mapType; //!< The kind of map representation (see CHeightGridMap2D::CHeightGridMap2D)
140 };
142
143 } // End of namespace
144
145 namespace global_settings
146 {
147 /** If set to true (default), mrpt::maps::CHeightGridMap2D will be exported as a opengl::CMesh, otherwise, as a opengl::CPointCloudColoured
148 * Affects to:
149 * - CHeightGridMap2D::getAs3DObject
150 */
151 extern MAPS_IMPEXP bool HEIGHTGRIDMAP_EXPORT3D_AS_MESH;
152 }
153
154 // Specializations MUST occur at the same namespace:
155 namespace utils
156 {
157 template <>
158 struct TEnumTypeFiller<maps::CHeightGridMap2D::TMapRepresentation>
159 {
161 static void fill(bimap<enum_t,std::string> &m_map)
162 {
163 m_map.insert(maps::CHeightGridMap2D::mrSimpleAverage, "mrSimpleAverage");
164 }
165 };
166 } // End of namespace
167
168} // End of namespace
169
170#endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#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...
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ... MAP_DEFINITION_END() block inside the declaration of each metric map...
Virtual base class for Digital Elevation Model (DEM) maps.
Digital Elevation Model (DEM), a mesh or grid representation of a surface which keeps the estimated h...
virtual bool insertIndividualPoint(const double x, const double y, const double z, const CHeightGridMap2D_Base::TPointInsertParams &params=CHeightGridMap2D_Base::TPointInsertParams()) MRPT_OVERRIDE
Update the DEM with one new point.
void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map: by default, it will be a mrpt::opengl::CMesh object,...
TMapRepresentation getMapType()
Return the type of the gas distribution map, according to parameters passed on construction.
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
void clear()
Calls the base CMetricMap::clear Declared here to avoid ambiguity between the two clear() in both bas...
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
See docs in base class: in this class it always returns 0.
size_t countObservedCells() const
Return the number of cells with at least one height data inserted.
float cell2float(const THeightGridmapCell &c) const MRPT_OVERRIDE
bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
TMapRepresentation
The type of map representation to be used.
CHeightGridMap2D(TMapRepresentation mapType=mrSimpleAverage, double x_min=-2, double x_max=2, double y_min=-2, double y_max=2, double resolution=0.1)
Constructor.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
This class allows loading and storing values and vectors of different types from a configuration text...
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:41
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:29
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
TColormap
Different colormaps.
Definition: color_maps.h:49
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
struct OPENGL_IMPEXP CSetOfObjectsPtr
Definition: CSetOfObjects.h:23
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
unsigned long uint32_t
Definition: pstdint.h:216
Parameters related with inserting observations into the map.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
bool filterByHeight
Whether to perform filtering by z-coordinate (default=false): coordinates are always RELATIVE to the ...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
float z_max
Only when filterByHeight is true: coordinates are always RELATIVE to the robot for this filter.
The contents of each cell in a CHeightGridMap2D map.
uint32_t w
[For mrSimpleAverage model] The accumulated weight: initially zero if un-observed,...
float u
Auxiliary variable for storing the incremental mean value (in meters).
float v
Auxiliary (in meters)
float var
The current standard deviation of the height (in meters)
float h
The current average height (in meters)
Parameters for CMetricMap::compute3DMatchingRatio()
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:24



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Wed Mar 22 09:54:56 UTC 2023