MRPT  1.9.9
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-2018, 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/config/CConfigFileBase.h> // MRPT_LOAD_CONFIG_VAR()
14 #include <mrpt/poses/CPose3D.h>
16 #include <mrpt/opengl/CMesh.h>
18 #include <mrpt/img/color_maps.h>
20 
21 using namespace mrpt::maps;
22 using namespace mrpt::obs;
23 using namespace mrpt::poses;
24 using namespace mrpt::img;
25 using namespace mrpt::math;
26 using namespace mrpt::system;
27 using namespace std;
28 
29 // =========== Begin of Map definition ============
31  "CHeightGridMap2D,heightMap,dem", mrpt::maps::CHeightGridMap2D)
32 
34  : min_x(-2),
35  max_x(2),
36  min_y(-2),
37  max_y(2),
38  resolution(0.10f),
39  mapType(CHeightGridMap2D::mrSimpleAverage)
40 {
41 }
42 
45  const std::string& sectionNamePrefix)
46 {
47  // [<sectionNamePrefix>+"_creationOpts"]
48  const std::string sSectCreation =
49  sectionNamePrefix + string("_creationOpts");
50  MRPT_LOAD_CONFIG_VAR(min_x, double, source, sSectCreation);
51  MRPT_LOAD_CONFIG_VAR(max_x, double, source, sSectCreation);
52  MRPT_LOAD_CONFIG_VAR(min_y, double, source, sSectCreation);
53  MRPT_LOAD_CONFIG_VAR(max_y, double, source, sSectCreation);
54  MRPT_LOAD_CONFIG_VAR(resolution, double, source, sSectCreation);
55  mapType = source.read_enum<CHeightGridMap2D::TMapRepresentation>(
56  sSectCreation, "mapType", mapType);
57 
58  insertionOpts.loadFromConfigFile(
59  source, sectionNamePrefix + string("_insertOpts"));
60 }
61 
63  std::ostream& out) const
64 {
65  LOADABLEOPTS_DUMP_VAR(min_x, double);
66  LOADABLEOPTS_DUMP_VAR(max_x, double);
67  LOADABLEOPTS_DUMP_VAR(min_y, double);
68  LOADABLEOPTS_DUMP_VAR(max_y, double);
69  LOADABLEOPTS_DUMP_VAR(resolution, double);
70  out << mrpt::format(
71  "MAP TYPE = %s\n",
73  CHeightGridMap2D::TMapRepresentation>::value2name(mapType)
74  .c_str());
75 
76  this->insertionOpts.dumpToTextStream(out);
77 }
78 
81 {
83  *dynamic_cast<const CHeightGridMap2D::TMapDefinition*>(&_def);
85  def.mapType, def.min_x, def.max_x, def.min_y, def.max_y,
86  def.resolution);
87  obj->insertionOptions = def.insertionOpts;
88  return obj;
89 }
90 // =========== End of Map definition Block =========
91 
93 
95 
96 bool mrpt::global_settings::HEIGHTGRIDMAP_EXPORT3D_AS_MESH()
97 {
99 }
101 {
103 }
104 
105 /*---------------------------------------------------------------
106  Constructor
107  ---------------------------------------------------------------*/
109  TMapRepresentation mapType, double x_min, double x_max, double y_min,
110  double y_max, double resolution)
111  : CDynamicGrid<THeightGridmapCell>(x_min, x_max, y_min, y_max, resolution),
112  insertionOptions(),
113  m_mapType(mapType)
114 {
115 }
116 
117 /*---------------------------------------------------------------
118  clear
119  ---------------------------------------------------------------*/
121 /*---------------------------------------------------------------
122  isEmpty
123  ---------------------------------------------------------------*/
124 bool CHeightGridMap2D::isEmpty() const { return false; }
126  const double x, const double y, const double z,
128 {
129  THeightGridmapCell* cell = cellByPos(x, y);
130  if (!cell)
131  return false; // Out of the map: Ignore if we've not resized before.
132 
135  {
136  cell->u += z;
137  cell->v += z * z;
138  if (!cell->w)
139  {
140  cell->h = z; // First observation
141  cell->w = 1;
142  }
143  else
144  {
145  float W = cell->w++; // W = N-1
146  cell->h = (cell->h * W + z) / cell->w;
147  if (W > 0)
148  cell->var = 1 / (W) * (cell->v - pow(cell->u, 2) / cell->w);
149  }
150  } // end if really inserted
151  return true;
152 }
153 
155  const CObservation* obs, const CPose3D* robotPose)
156 {
157  return dem_internal_insertObservation(obs, robotPose);
158 }
159 
160 /*---------------------------------------------------------------
161  computeObservationLikelihood
162  ---------------------------------------------------------------*/
164  const CObservation* obs, const CPose3D& takenFrom)
165 {
166  MRPT_UNUSED_PARAM(obs);
167  MRPT_UNUSED_PARAM(takenFrom);
168 
169  THROW_EXCEPTION("Not implemented yet!");
170 }
171 
174 {
176 
177  // To assure compatibility: The size of each cell:
178  uint32_t n = static_cast<uint32_t>(sizeof(THeightGridmapCell));
179  out << n;
180 
181  // Save the map contents:
182  n = static_cast<uint32_t>(m_map.size());
183  out << n;
185  it != m_map.end(); ++it)
186  out << it->h
187  << it->w; // This was removed in version 1: << it->history_Zs;
188 
189  // Save the insertion options:
190  out << uint8_t(m_mapType);
191 
194 
195  out << genericMapParams; // v2
196 }
197 
200 {
201  switch (version)
202  {
203  case 0:
204  case 1:
205  case 2:
206  case 3:
207  {
208  dyngridcommon_readFromStream(in, version < 3);
209  // To ensure compatibility: The size of each cell:
210  uint32_t n;
211  in >> n;
212  ASSERT_(n == static_cast<uint32_t>(sizeof(THeightGridmapCell)));
213 
214  // Save the map contents:
215  in >> n;
216  m_map.resize(n);
218  it != m_map.end(); ++it)
219  {
220  in >> it->h >> it->w;
221  // Data member in version 0:
222  if (version == 0)
223  {
224  std::multimap<mrpt::system::TTimeStamp, float> history_Zs;
225  in >> history_Zs; // Discarded now...
226  }
227  }
228 
229  // Insertion options:
230  uint8_t ty;
231  in >> ty;
233 
236 
237  if (version >= 2) in >> genericMapParams;
238  }
239  break;
240  default:
242  };
243 }
244 
246  : filterByHeight(false), z_min(-0.5), z_max(0.5), colorMap(cmJET)
247 {
248 }
249 
251  std::ostream& out) const
252 {
253  out << mrpt::format(
254  "\n----------- [CHeightGridMap2D::TInsertionOptions] ------------ "
255  "\n\n");
256  out << mrpt::format(
257  "filterByHeight = %c\n",
258  filterByHeight ? 'y' : 'n');
259  out << mrpt::format(
260  "z_min = %f\n", z_min);
261  out << mrpt::format(
262  "z_max = %f\n", z_max);
263  out << mrpt::format(
264  "colormap = %s\n",
265  colorMap == cmJET ? "jet" : "grayscale");
266  out << mrpt::format("\n");
267 }
268 
269 /*---------------------------------------------------------------
270  loadFromConfigFile
271  ---------------------------------------------------------------*/
273  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
274 {
275  MRPT_LOAD_CONFIG_VAR(filterByHeight, bool, iniFile, section)
276  MRPT_LOAD_CONFIG_VAR(z_min, float, iniFile, section)
277  MRPT_LOAD_CONFIG_VAR(z_max, float, iniFile, section)
278  string aux = iniFile.read_string(section, "colorMap", "jet");
279 
280  if (strCmp(aux, "jet"))
281  colorMap = cmJET;
282  else if (strCmp(aux, "grayscale"))
283  colorMap = cmGRAYSCALE;
284 }
285 
286 /*---------------------------------------------------------------
287  saveMetricMapRepresentationToFile
288  ---------------------------------------------------------------*/
290  const std::string& filNamePrefix) const
291 {
292  // Text matrix:
293  saveToTextFile(filNamePrefix + std::string("_mean.txt"));
294 }
295 
296 /*---------------------------------------------------------------
297  getAs3DObject
298 ---------------------------------------------------------------*/
300  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
301 {
303 
305  {
306  opengl::CMesh::Ptr mesh = mrpt::make_aligned_shared<opengl::CMesh>();
307 
308  mesh->setGridLimits(m_x_min, m_x_max, m_y_min, m_y_max);
309 
310  mesh->setColor(0.4, 0.4, 0.4);
311 
312  mesh->enableWireFrame(true);
313  mesh->enableColorFromZ(true, insertionOptions.colorMap /*cmJET*/);
314 
315  CMatrixFloat Z, mask;
316  /*mesh->getZ(Z);
317 
318  mesh->getMask(mask);*/
319 
320  Z.setSize(m_size_x, m_size_y);
321  mask.setSize(m_size_x, m_size_y);
322 
323  for (size_t x = 0; x < m_size_x; x++)
324  {
325  for (size_t y = 0; y < m_size_y; y++)
326  {
327  const THeightGridmapCell* c = cellByIndex(x, y);
328  ASSERTDEB_(c);
329  Z.set_unsafe(x, y, c->h);
330  mask.set_unsafe(x, y, c->w ? 1 : 0);
331  }
332  }
333  mesh->setZ(Z);
334  mesh->setMask(mask);
335 
336  outObj->insert(mesh);
337  }
338  else
339  {
340  // As points:
342  mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
343  obj->setPointSize(2);
344 
345  // Find min/max:
346  float z_min, z_max;
347  float K;
348  if (this->getMinMaxHeight(z_min, z_max))
349  K = 1.0f / (z_max - z_min);
350  else
351  K = 1.0f;
352 
353  obj->reserve(m_size_x * m_size_y);
354  for (size_t x = 0; x < m_size_x; x++)
355  for (size_t y = 0; y < m_size_y; y++)
356  {
357  const THeightGridmapCell* c = cellByIndex(x, y);
358  ASSERTDEB_(c);
359  if (c->w)
360  {
361  float r, g, b;
362  const float col_idx = (c->h - z_min) * K;
363  colormap(
364  insertionOptions.colorMap, // cmJET, //cmGRAYSCALE,
365  col_idx, r, g, b);
366  obj->push_back(idx2x(x), idx2y(y), c->h, r, g, b);
367  }
368  }
369 
370  outObj->insert(obj);
371  }
372 }
373 
374 /** Return the number of cells with at least one height data inserted. */
376 {
377  switch (m_mapType)
378  {
379  case mrSimpleAverage:
380  {
381  size_t obsCells = 0;
382  const size_t N = m_map.size();
383  for (size_t i = 0; i < N; i++)
384  if (m_map[i].w) obsCells++;
385  return obsCells;
386  }
387  break;
388  default:
390  "countObservedCells() not implemented for this mapType (!?)")
391  };
392 }
393 
395 size_t CHeightGridMap2D::dem_get_size_x() const { return m_size_x; }
396 size_t CHeightGridMap2D::dem_get_size_y() const { return m_size_y; }
398  const size_t cx, const size_t cy, double& z_out) const
399 {
400  const THeightGridmapCell* cell = cellByIndex(cx, cy);
401  if (cell && cell->w)
402  {
403  z_out = cell->h;
404  return true;
405  }
406  else
407  return false;
408 }
410  const double x, const double y, double& z_out) const
411 {
412  const THeightGridmapCell* cell = cellByPos(x, y);
413  if (cell && cell->w)
414  {
415  z_out = cell->h;
416  return true;
417  }
418  else
419  {
420  return false;
421  }
422 }
424 {
425  // Nothing to do in this class: estimate is always up-to-date
426 }
427 
429  const mrpt::maps::CMetricMap* otherMap,
430  const mrpt::poses::CPose3D& otherMapPose,
431  const TMatchingRatioParams& params) const
432 {
433  MRPT_UNUSED_PARAM(otherMap);
434  MRPT_UNUSED_PARAM(otherMapPose);
436  return 0;
437 }
Digital Elevation Model (DEM), a mesh or grid representation of a surface which keeps the estimated h...
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...
Scalar * iterator
Definition: eigen_plugins.h:26
Parameters for CMetricMap::compute3DMatchingRatio()
std::vector< THeightGridmapCell > m_map
The cells.
Definition: CDynamicGrid.h:42
void 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:113
GLenum GLint GLuint mask
Definition: glext.h:4050
Extra params for insertIndividualPoint()
GLdouble GLdouble z
Definition: glext.h:3872
void fill(const THeightGridmapCell &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:117
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
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.
virtual void dem_update_map() override
Ensure that all observations are reflected in the map estimate.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
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.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
See docs in base class: in this class it always returns 0.
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
GLenum GLsizei n
Definition: glext.h:5074
The contents of each cell in a CHeightGridMap2D map.
virtual bool dem_get_z_by_cell(const size_t cx, const size_t cy, double &z_out) const override
Get cell &#39;z&#39; by (cx,cy) cell indices.
virtual bool dem_get_z(const double x, const double y, double &z_out) const override
Get cell &#39;z&#39; (x,y) by metric coordinates.
bool strCmp(const std::string &s1, const std::string &s2)
Return true if the two strings are equal (case sensitive)
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:280
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:289
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:4070
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
mrpt::maps::CHeightGridMap2D::TInsertionOptions insertionOptions
virtual size_t dem_get_size_x() const override
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
virtual bool insertIndividualPoint(const double x, const double y, const double z, const CHeightGridMap2D_Base::TPointInsertParams &params=CHeightGridMap2D_Base::TPointInsertParams()) override
Update the DEM with one new point.
float z_min
Only when filterByHeight is true: coordinates are always RELATIVE to the robot for this filter...
A helper class that can convert an enum value into its textual representation, and viceversa...
const GLubyte * c
Definition: glext.h:6313
THeightGridmapCell * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or nullptr if it is out of the ...
Definition: CDynamicGrid.h:211
GLbyte ty
Definition: glext.h:6092
bool dem_internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Internal method called by internal_insertObservation()
GLubyte g
Definition: glext.h:6279
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
Undefined colormap [New in MRPT 2.0].
Definition: color_maps.h:32
GLubyte GLubyte b
Definition: glext.h:6279
virtual double dem_get_resolution() const override
THeightGridmapCell * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
Definition: CDynamicGrid.h:232
float u
Auxiliary variable for storing the incremental mean value (in meters).
void dyngridcommon_readFromStream(STREAM &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:351
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
void dumpToTextStream_map_specific(std::ostream &out) const override
#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...
mrpt::maps::CHeightGridMap2D::TMapRepresentation mapType
The kind of map representation (see CHeightGridMap2D::CHeightGridMap2D)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:52
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
virtual size_t dem_get_size_y() const override
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:54
bool filterByHeight
Whether to perform filtering by z-coordinate (default=false): coordinates are always RELATIVE to the ...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:86
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
void internal_clear() override
Internal method called by clear()
GLuint in
Definition: glext.h:7274
bool saveToTextFile(const std::string &fileName) const
Saves a float representation of the grid (via "cell2float()") to a text file.
Definition: CDynamicGrid.h:323
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map: by default, it will be a mrpt::opengl::CMesh object...
TMapRepresentation m_mapType
The map representation type of this map.
GLenum GLint GLint y
Definition: glext.h:3538
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
GLsizei const GLfloat * value
Definition: glext.h:4117
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
mrpt::maps::CHeightGridMap2D::TInsertionOptions insertionOpts
GLenum GLint x
Definition: glext.h:3538
double min_x
See CHeightGridMap2D::CHeightGridMap2D.
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
unsigned __int32 uint32_t
Definition: rptypes.h:47
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.
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
GLenum const GLfloat * params
Definition: glext.h:3534
const Scalar * const_iterator
Definition: eigen_plugins.h:27
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void HEIGHTGRIDMAP_EXPORT3D_AS_MESH(bool value)
If set to true (default), mrpt::maps::CHeightGridMap2D will be exported as a opengl::CMesh, otherwise, as a opengl::CPointCloudColoured Affects to:
bool HEIGHTGRIDMAP_EXPORT3D_AS_MESH_value
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020