Main MRPT website > C++ reference for MRPT 1.5.7
CHeightGridMap2D.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, 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 #include "maps-precomp.h" // Precomp header
11 
13 #include <mrpt/utils/CConfigFileBase.h> // MRPT_LOAD_CONFIG_VAR()
14 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/opengl/CMesh.h>
18 
19 //#include <mrpt/system/os.h>
20 //#include <mrpt/utils/color_maps.h>
21 #include <mrpt/utils/CStream.h>
22 
23 using namespace mrpt::maps;
24 using namespace mrpt::obs;
25 using namespace mrpt::poses;
26 using namespace mrpt::math;
27 using namespace mrpt::utils;
28 using namespace mrpt::system;
29 using namespace std;
30 
31 
32 // =========== Begin of Map definition ============
33 MAP_DEFINITION_REGISTER("CHeightGridMap2D,heightMap,dem", mrpt::maps::CHeightGridMap2D)
34 
36  min_x(-2),
37  max_x(2),
38  min_y(-2),
39  max_y(2),
40  resolution(0.10f),
41  mapType(CHeightGridMap2D::mrSimpleAverage)
42 {
43 }
44 
46 {
47  // [<sectionNamePrefix>+"_creationOpts"]
48  const std::string sSectCreation = sectionNamePrefix+string("_creationOpts");
49  MRPT_LOAD_CONFIG_VAR(min_x, double, source,sSectCreation);
50  MRPT_LOAD_CONFIG_VAR(max_x, double, source,sSectCreation);
51  MRPT_LOAD_CONFIG_VAR(min_y, double, source,sSectCreation);
52  MRPT_LOAD_CONFIG_VAR(max_y, double, source,sSectCreation);
53  MRPT_LOAD_CONFIG_VAR(resolution, double, source,sSectCreation);
54  mapType = source.read_enum<CHeightGridMap2D::TMapRepresentation>(sSectCreation,"mapType",mapType);
55 
56  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
57 }
58 
60 {
61  LOADABLEOPTS_DUMP_VAR(min_x , double);
62  LOADABLEOPTS_DUMP_VAR(max_x , double);
63  LOADABLEOPTS_DUMP_VAR(min_y , double);
64  LOADABLEOPTS_DUMP_VAR(max_y , double);
65  LOADABLEOPTS_DUMP_VAR(resolution , double);
67 
68  this->insertionOpts.dumpToTextStream(out);
69 }
70 
72 {
73  const CHeightGridMap2D::TMapDefinition &def = *dynamic_cast<const CHeightGridMap2D::TMapDefinition*>(&_def);
75  obj->insertionOptions = def.insertionOpts;
76  return obj;
77 }
78 // =========== End of Map definition Block =========
79 
80 
82 
83 bool mrpt::global_settings::HEIGHTGRIDMAP_EXPORT3D_AS_MESH = true;
84 
85 /*---------------------------------------------------------------
86  Constructor
87  ---------------------------------------------------------------*/
89  TMapRepresentation mapType,
90  double x_min, double x_max,
91  double y_min, double y_max,
92  double resolution ) :
93  CDynamicGrid<THeightGridmapCell>( x_min,x_max,y_min,y_max,resolution ),
94  insertionOptions(),
95  m_mapType(mapType)
96 {
97 }
98 
99 /*---------------------------------------------------------------
100  clear
101  ---------------------------------------------------------------*/
103 {
105 }
106 
107 /*---------------------------------------------------------------
108  isEmpty
109  ---------------------------------------------------------------*/
111 {
112  return false;
113 }
114 
116 {
117  THeightGridmapCell *cell = cellByPos(x,y);
118  if(!cell) return false; // Out of the map: Ignore if we've not resized before.
119 
120  if (!insertionOptions.filterByHeight || (z>=insertionOptions.z_min && z<=insertionOptions.z_max ) )
121  {
122  cell->u += z;
123  cell->v += z*z;
124  if (!cell->w)
125  {
126  cell->h = z; // First observation
127  cell->w = 1;
128  }
129  else
130  {
131  float W = cell->w++; // W = N-1
132  cell->h = (cell->h*W + z)/cell->w;
133  if (W > 0)
134  cell->var = 1/(W) * (cell->v - pow(cell->u,2)/cell->w);
135  }
136  } // end if really inserted
137  return true;
138 }
139 
141 {
142  return dem_internal_insertObservation(obs,robotPose);
143 }
144 
145 
146 /*---------------------------------------------------------------
147  computeObservationLikelihood
148  ---------------------------------------------------------------*/
150  const CObservation *obs,
151  const CPose3D &takenFrom )
152 {
153  MRPT_UNUSED_PARAM(obs);MRPT_UNUSED_PARAM(takenFrom);
154 
155  THROW_EXCEPTION("Not implemented yet!");
156 }
157 
158 /*---------------------------------------------------------------
159  Implements the writing to a CStream capability of CSerializable objects
160  ---------------------------------------------------------------*/
162 {
163  if (version)
164  *version = 3;
165  else
166  {
167  dyngridcommon_writeToStream(out);
168 
169  // To assure compatibility: The size of each cell:
170  uint32_t n = static_cast<uint32_t>(sizeof( THeightGridmapCell ));
171  out << n;
172 
173  // Save the map contents:
174  n = static_cast<uint32_t>(m_map.size());
175  out << n;
176  for (vector<THeightGridmapCell>::const_iterator it=m_map.begin();it!=m_map.end();++it)
177  out << it->h << it->w; // This was removed in version 1: << it->history_Zs;
178 
179  // Save the insertion options:
180  out << uint8_t(m_mapType);
181 
182  out << insertionOptions.filterByHeight
183  << insertionOptions.z_min
184  << insertionOptions.z_max;
185 
186  out << genericMapParams; // v2
187  }
188 }
189 
190 /*---------------------------------------------------------------
191  Implements the reading from a CStream capability of CSerializable objects
192  ---------------------------------------------------------------*/
194 {
195  switch(version)
196  {
197  case 0:
198  case 1:
199  case 2:
200  case 3:
201  {
202  dyngridcommon_readFromStream(in, version<3);
203  // To ensure compatibility: The size of each cell:
204  uint32_t n;
205  in >> n;
206  ASSERT_( n == static_cast<uint32_t>( sizeof( THeightGridmapCell ) ));
207 
208  // Save the map contents:
209  in >> n;
210  m_map.resize(n);
211  for (vector<THeightGridmapCell>::iterator it=m_map.begin();it!=m_map.end();++it)
212  {
213  in >> it->h >> it->w;
214  // Data member in version 0:
215  if (version==0)
216  {
217  std::multimap<mrpt::system::TTimeStamp,float> history_Zs;
218  in >> history_Zs; // Discarded now...
219  }
220  }
221 
222  // Insertion options:
223  uint8_t ty;
224  in >> ty;
225  m_mapType = TMapRepresentation(ty);
226 
227  in >> insertionOptions.filterByHeight
228  >> insertionOptions.z_min
229  >> insertionOptions.z_max;
230 
231  if (version>=2)
232  in >> genericMapParams;
233 
234  } break;
235  default:
237 
238  };
239 
240 }
241 
242 /*---------------------------------------------------------------
243  TInsertionOptions
244  ---------------------------------------------------------------*/
246  filterByHeight ( false ),
247  z_min ( -0.5 ),
248  z_max ( 0.5 ),
249  colorMap( cmJET )
250 {
251 }
252 
253 /*---------------------------------------------------------------
254  dumpToTextStream
255  ---------------------------------------------------------------*/
257 {
258  out.printf("\n----------- [CHeightGridMap2D::TInsertionOptions] ------------ \n\n");
259  out.printf("filterByHeight = %c\n", filterByHeight ? 'y':'n');
260  out.printf("z_min = %f\n", z_min);
261  out.printf("z_max = %f\n", z_max);
262  out.printf("colormap = %s\n", colorMap == cmJET ? "jet" : "grayscale");
263  out.printf("\n");
264 }
265 
266 /*---------------------------------------------------------------
267  loadFromConfigFile
268  ---------------------------------------------------------------*/
270  const mrpt::utils::CConfigFileBase &iniFile,
271  const std::string &section)
272 {
273  MRPT_LOAD_CONFIG_VAR( filterByHeight, bool, iniFile, section )
274  MRPT_LOAD_CONFIG_VAR( z_min, float, iniFile, section )
275  MRPT_LOAD_CONFIG_VAR( z_max, float, iniFile, section )
276  string aux = iniFile.read_string(section, "colorMap", "jet");
277 
278  if(strCmp(aux,"jet") )
279  colorMap = cmJET;
280  else if(strCmp(aux,"grayscale") )
281  colorMap = cmGRAYSCALE;
282 }
283 
284 /*---------------------------------------------------------------
285  saveMetricMapRepresentationToFile
286  ---------------------------------------------------------------*/
288  const std::string &filNamePrefix ) const
289 {
290  // Text matrix:
291  saveToTextFile( filNamePrefix + std::string("_mean.txt") );
292 }
293 
294 /*---------------------------------------------------------------
295  getAs3DObject
296 ---------------------------------------------------------------*/
297 void CHeightGridMap2D::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const
298 {
300 
302  {
303  opengl::CMeshPtr mesh = opengl::CMesh::Create();
304 
305  mesh->setGridLimits(m_x_min,m_x_max, m_y_min, m_y_max);
306 
307  mesh->setColor(0.4, 0.4, 0.4 );
308 
309  mesh->enableWireFrame(true);
310  mesh->enableColorFromZ(true, insertionOptions.colorMap /*cmJET*/);
311 
312  CMatrixFloat Z,mask;
313  /*mesh->getZ(Z);
314 
315  mesh->getMask(mask);*/
316 
317  Z.setSize( m_size_x, m_size_y );
318  mask.setSize( m_size_x, m_size_y );
319 
320  for (size_t x=0;x<m_size_x;x++)
321  {
322  for (size_t y=0;y<m_size_y;y++)
323  {
324  const THeightGridmapCell *c = cellByIndex(x,y);
325  ASSERTDEB_(c);
326  Z.set_unsafe( x, y, c->h );
327  mask.set_unsafe( x, y, c->w ? 1:0 );
328  }
329  }
330  mesh->setZ(Z);
331  mesh->setMask(mask);
332 
333  outObj->insert( mesh );
334  }
335  else
336  {
337  // As points:
338  mrpt::opengl::CPointCloudColouredPtr obj = mrpt::opengl::CPointCloudColoured::Create();
339  obj->setPointSize(2);
340 
341  // Find min/max:
342  float z_min,z_max;
343  float K;
344  if (this->getMinMaxHeight(z_min,z_max))
345  K = 1.0f/(z_max-z_min);
346  else K = 1.0f;
347 
348  obj->reserve(m_size_x*m_size_y);
349  for (size_t x=0;x<m_size_x;x++)
350  for (size_t y=0;y<m_size_y;y++)
351  {
352  const THeightGridmapCell *c = cellByIndex(x,y);
353  ASSERTDEB_(c)
354  if (c->w)
355  {
356  float r,g,b;
357  const float col_idx = (c->h-z_min)*K;
358  colormap(
359  insertionOptions.colorMap, //cmJET, //cmGRAYSCALE,
360  col_idx, r,g,b );
361  obj->push_back( idx2x(x),idx2y(y), c->h, r,g,b );
362  }
363  }
364 
365  outObj->insert( obj );
366  }
367 }
368 
369 /** Return the number of cells with at least one height data inserted. */
371 {
372  switch (m_mapType)
373  {
374  case mrSimpleAverage:
375  {
376  size_t obsCells = 0;
377  const size_t N = m_map.size();
378  for (size_t i=0;i<N;i++)
379  if (m_map[i].w)
380  obsCells++;
381  return obsCells;
382  }
383  break;
384  default: THROW_EXCEPTION("countObservedCells() not implemented for this mapType (!?)")
385  };
386 }
387 
389  return m_resolution;
390 }
392  return m_size_x;
393 }
395  return m_size_y;
396 }
397 bool CHeightGridMap2D::dem_get_z_by_cell(const size_t cx, const size_t cy, double &z_out) const {
398  const THeightGridmapCell *cell = cellByIndex(cx,cy);
399  if (cell && cell->w) {
400  z_out = cell->h;
401  return true;
402  } else return false;
403 }
404 bool CHeightGridMap2D::dem_get_z(const double x, const double y, double &z_out) const
405 {
406  const THeightGridmapCell *cell = cellByPos(x,y);
407  if (cell && cell->w) {
408  z_out = cell->h;
409  return true;
410  } else {
411  return false;
412  }
413 }
415  // Nothing to do in this class: estimate is always up-to-date
416 }
417 
419 {
420  MRPT_UNUSED_PARAM(otherMap); MRPT_UNUSED_PARAM(otherMapPose);
422  return 0;
423 }
424 
425 
Digital Elevation Model (DEM), a mesh or grid representation of a surface which keeps the estimated h...
void loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
Load all map-specific params.
float var
The current standard deviation of the height (in meters)
float h
The current average height (in meters)
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Parameters for CMetricMap::compute3DMatchingRatio()
GLenum GLint GLuint mask
Definition: glext.h:3888
Extra params for insertIndividualPoint()
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLdouble GLdouble z
Definition: glext.h:3734
EIGEN_STRONG_INLINE void fill(const Scalar v)
Definition: eigen_plugins.h:38
virtual double dem_get_resolution() const MRPT_OVERRIDE
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
TMapRepresentation
The type of map representation to be used.
#define THROW_EXCEPTION(msg)
GLenum GLsizei n
Definition: glext.h:4618
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.
Scalar * iterator
Definition: eigen_plugins.h:23
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
The contents of each cell in a CHeightGridMap2D map.
void dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
bool BASE_IMPEXP strCmp(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case sensitive)
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
void BASE_IMPEXP colormap(const TColormap &color_map, const float color_index, float &r, float &g, float &b)
Transform a float number in the range [0,1] into RGB components.
Definition: color_maps.cpp:101
uint32_t w
[For mrSimpleAverage model] The accumulated weight: initially zero if un-observed, increased by one for each observation
float v
Auxiliary (in meters)
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:3962
This class allows loading and storing values and vectors of different types from a configuration text...
unsigned char uint8_t
Definition: rptypes.h:43
THeightGridmapCell * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map...
Definition: CDynamicGrid.h:183
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:
virtual void dem_update_map() MRPT_OVERRIDE
Ensure that all observations are reflected in the map estimate.
mrpt::maps::CHeightGridMap2D::TInsertionOptions insertionOptions
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:40
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
A helper class that can convert an enum value into its textual representation, and viceversa...
const GLubyte * c
Definition: glext.h:5590
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL) MRPT_OVERRIDE
Internal method called by insertObservation()
std::vector< THeightGridmapCell > m_map
The cells.
Definition: CDynamicGrid.h:43
GLbyte ty
Definition: glext.h:5441
virtual size_t dem_get_size_y() const MRPT_OVERRIDE
GLubyte g
Definition: glext.h:5575
This namespace contains representation of robot actions and observations.
GLubyte GLubyte b
Definition: glext.h:5575
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...
int version
Definition: mrpt_jpeglib.h:898
float u
Auxiliary variable for storing the incremental mean value (in meters).
GLsizei const GLchar ** string
Definition: glext.h:3919
THeightGridmapCell * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the ma...
Definition: CDynamicGrid.h:203
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:253
bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
virtual size_t dem_get_size_x() const MRPT_OVERRIDE
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::maps::CHeightGridMap2D::TMapRepresentation mapType
The kind of map representation (see CHeightGridMap2D::CHeightGridMap2D)
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
virtual bool dem_get_z(const double x, const double y, double &z_out) const MRPT_OVERRIDE
Get cell &#39;z&#39; (x,y) by metric coordinates.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
GLdouble GLdouble GLdouble r
Definition: glext.h:3618
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
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:72
Declares a class that represents any robot&#39;s observation.
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:280
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
GLuint in
Definition: glext.h:6301
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
#define ASSERT_(f)
TMapRepresentation m_mapType
The map representation type of this map.
GLenum GLint GLint y
Definition: glext.h:3516
static CMeshPtr Create()
mrpt::maps::CHeightGridMap2D::TInsertionOptions insertionOpts
GLenum GLint x
Definition: glext.h:3516
unsigned __int32 uint32_t
Definition: rptypes.h:49
size_t countObservedCells() const
Return the number of cells with at least one height data inserted.
bool getMinMaxHeight(float &z_min, float &z_max) const
Computes the minimum and maximum height in the grid.
double resolution
See CHeightGridMap2D::CHeightGridMap2D.
GLenum const GLfloat * params
Definition: glext.h:3514
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.
virtual bool dem_get_z_by_cell(const size_t cx, const size_t cy, double &z_out) const MRPT_OVERRIDE
Get cell &#39;z&#39; by (cx,cy) cell indices.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
static CPointCloudColouredPtr Create()



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019