MRPT  1.9.9
COccupancyGridMap3D.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
16 #include <mrpt/poses/CPose3D.h>
18 
19 using namespace mrpt;
20 using namespace mrpt::maps;
21 
22 // =========== Begin of Map definition ============
24  "mrpt::maps::COccupancyGridMap3D", mrpt::maps::COccupancyGridMap3D)
25 
27 
30 {
31  using namespace std::string_literals;
32 
33  // [<sect>+"_creationOpts"]
34  const auto sSectCreation = sect + "_creationOpts"s;
35  MRPT_LOAD_CONFIG_VAR(min_x, float, source, sSectCreation);
36  MRPT_LOAD_CONFIG_VAR(max_x, float, source, sSectCreation);
37  MRPT_LOAD_CONFIG_VAR(min_y, float, source, sSectCreation);
38  MRPT_LOAD_CONFIG_VAR(max_y, float, source, sSectCreation);
39  MRPT_LOAD_CONFIG_VAR(min_z, float, source, sSectCreation);
40  MRPT_LOAD_CONFIG_VAR(max_z, float, source, sSectCreation);
41  MRPT_LOAD_CONFIG_VAR(resolution, float, source, sSectCreation);
42 
43  // [<sectionName>+"_occupancyGrid_##_insertOpts"]
44  insertionOpts.loadFromConfigFile(source, sect + "_insertOpts"s);
45 
46  // [<sectionName>+"_occupancyGrid_##_likelihoodOpts"]
47  likelihoodOpts.loadFromConfigFile(source, sect + "_likelihoodOpts"s);
48 }
49 
51  std::ostream& out) const
52 {
60 
63 }
64 
67 {
68  auto& def = dynamic_cast<const COccupancyGridMap3D::TMapDefinition&>(_def);
69  auto* obj = new COccupancyGridMap3D(
70  mrpt::math::TPoint3D(def.min_x, def.min_y, def.min_z),
71  mrpt::math::TPoint3D(def.max_x, def.max_y, def.max_z), def.resolution);
72  obj->insertionOptions = def.insertionOpts;
73  obj->likelihoodOptions = def.likelihoodOpts;
74  return obj;
75 }
76 // =========== End of Map definition Block =========
77 
79 
80 // Static lookup tables for log-odds
82 
85 {
86  return logodd_lut;
87 }
88 
90  const mrpt::math::TPoint3D& corner_min,
91  const mrpt::math::TPoint3D& corner_max, float resolution)
92 {
94  setSize(corner_min, corner_max, resolution, 0.5f);
95  MRPT_END
96 }
97 
99  const mrpt::math::TPoint3D& cmin, const mrpt::math::TPoint3D& cmax,
100  float res, float default_value)
101 {
102  MRPT_START
103 
104  ASSERT_ABOVE_(res, 0.0f);
105  ASSERT_ABOVE_(cmax.x, cmin.x);
106  ASSERT_ABOVE_(cmax.y, cmin.y);
107  ASSERT_ABOVE_(cmax.z, cmin.z);
108  ASSERT_ABOVEEQ_(default_value, 0.0f);
109  ASSERT_BELOWEQ_(default_value, 1.0f);
110 
111  const auto def_value = p2l(default_value);
112  m_grid.setSize(
113  cmin.x, cmax.x, cmin.y, cmax.y, cmin.z, cmax.z, res, res, &def_value);
114 
115  // m_likelihoodCacheOutDated = true;
116  m_is_empty = true;
117 
118  MRPT_END
119 }
120 
122  const mrpt::math::TPoint3D& cmin, const mrpt::math::TPoint3D& cmax,
123  float new_cells_default_value)
124 {
125  MRPT_START
126 
127  const auto def_value = p2l(new_cells_default_value);
128  const double additionalMargin = .0;
129  m_grid.resize(
130  cmin.x, cmax.x, cmin.y, cmax.y, cmin.z, cmax.z, def_value,
131  additionalMargin);
132 
133  // m_likelihoodCacheOutDated = true;
134  m_is_empty = true;
135 
136  MRPT_END
137 }
138 
140 {
141  TMapDefinition md;
142 
143  setSize(
147 
148  // m_likelihoodCacheOutDated = true;
149  m_is_empty = true;
150 }
151 
152 void COccupancyGridMap3D::fill(float default_value)
153 {
154  const voxelType defValue = p2l(default_value);
155  m_grid.fill(defValue);
156  // m_likelihoodCacheOutDated = true;
157 }
158 
159 void COccupancyGridMap3D::updateCell(int x, int y, int z, float v)
160 {
161  if (m_grid.isOutOfBounds(x, y, z)) return;
162 
163  // Get the current contents of the cell:
164  auto* cp = m_grid.cellByIndex(x, y, z);
165  ASSERT_(cp != nullptr);
166  voxelType& theCell = *cp;
167 
168  // Compute the new Bayesian-fused value of the cell:
169  // The observation: will be >0 for free, <0 for occupied.
170  const voxelType obs = p2l(v);
171  if (obs > 0)
172  {
173  // Saturate
174  if (theCell > (CLogOddsGridMap3D<voxelType>::CELLTYPE_MAX - obs))
176  else
177  theCell += obs;
178  }
179  else
180  {
181  // Saturate
182  if (theCell < (CLogOddsGridMap3D<voxelType>::CELLTYPE_MIN - obs))
184  else
185  theCell += obs;
186  }
187 }
188 
190  const mrpt::maps::CMetricMap* otherMap2,
191  const mrpt::poses::CPose2D& otherMapPose_,
192  mrpt::tfest::TMatchingPairList& correspondences,
194  mrpt::maps::TMatchingExtraResults& extraResults) const
195 {
196  MRPT_START
197 
198  THROW_EXCEPTION("Implement me!");
199 
200  MRPT_END
201 }
202 
203 bool COccupancyGridMap3D::isEmpty() const { return m_is_empty; }
204 
206  const mrpt::maps::CMetricMap* otherMap,
207  const mrpt::poses::CPose3D& otherMapPose,
208  const TMatchingRatioParams& params) const
209 {
210  MRPT_UNUSED_PARAM(otherMap);
211  MRPT_UNUSED_PARAM(otherMapPose);
213  THROW_EXCEPTION("Implement me!");
214  return 0;
215 }
216 
218  mrpt::opengl::COctoMapVoxels& gl_obj) const
219 {
220  MRPT_START
221 
222  using mrpt::img::TColor;
223  using mrpt::img::TColorf;
224  using namespace mrpt::opengl;
225 
226  const size_t N = m_grid.getSizeX() * m_grid.getSizeY() * m_grid.getSizeZ();
227  const TColorf general_color = gl_obj.getColor();
228  const TColor general_color_u(
229  general_color.R * 255, general_color.G * 255, general_color.B * 255,
230  general_color.A * 255);
231 
232  gl_obj.clear();
233  gl_obj.resizeVoxelSets(2); // 2 sets of voxels: occupied & free
235 
236  gl_obj.showVoxels(
239  gl_obj.showVoxels(
241 
244 
245  const mrpt::math::TPoint3D bbmin(
247  const mrpt::math::TPoint3D bbmax(
249  const auto inv_dz = 1.0 / (bbmax.z - bbmin.z + 0.01);
250  const double L = 0.5 * m_grid.getResolutionZ();
251 
252  for (size_t cz = 0; cz < m_grid.getSizeZ(); cz++)
253  {
254  // voxel center coordinates:
255  const double z = m_grid.idx2z(cz) + m_grid.getResolutionZ() * 0.5;
256  for (size_t cy = 0; cy < m_grid.getSizeY(); cy++)
257  {
258  const double y = m_grid.idx2y(cy) + m_grid.getResolutionXY() * 0.5;
259  for (size_t cx = 0; cx < m_grid.getSizeX(); cx++)
260  {
261  const double x =
262  m_grid.idx2x(cx) + m_grid.getResolutionXY() * 0.5;
263  const float occ = 1.0f - this->getCellFreeness(cx, cy, cz);
264  const bool is_occupied = occ > 0.501f;
265  const bool is_free = occ < 0.499f;
266  if ((is_occupied && renderingOptions.generateOccupiedVoxels) ||
268  {
269  mrpt::img::TColor vx_color;
270  double coefc, coeft;
271  switch (gl_obj.getVisualizationMode())
272  {
273  case COctoMapVoxels::FIXED:
274  vx_color = general_color_u;
275  break;
276  case COctoMapVoxels::COLOR_FROM_HEIGHT:
277  coefc = 255 * inv_dz * (z - bbmin.z);
278  vx_color = TColor(
279  coefc * general_color.R,
280  coefc * general_color.G,
281  coefc * general_color.B,
282  255.0 * general_color.A);
283  break;
284 
285  case COctoMapVoxels::COLOR_FROM_OCCUPANCY:
286  coefc = 240 * (1 - occ) + 15;
287  vx_color = TColor(
288  coefc * general_color.R,
289  coefc * general_color.G,
290  coefc * general_color.B,
291  255.0 * general_color.A);
292  break;
293 
294  case COctoMapVoxels::TRANSPARENCY_FROM_OCCUPANCY:
295  coeft = 255 - 510 * (1 - occ);
296  if (coeft < 0)
297  {
298  coeft = 0;
299  }
300  vx_color = TColor(
301  255 * general_color.R, 255 * general_color.G,
302  255 * general_color.B, coeft);
303  break;
304 
305  case COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY:
306  coefc = 240 * (1 - occ) + 15;
307  vx_color = TColor(
308  coefc * general_color.R,
309  coefc * general_color.G,
310  coefc * general_color.B, 50);
311  break;
312 
313  case COctoMapVoxels::MIXED:
314  coefc = 255 * inv_dz * (z - bbmin.z);
315  coeft = 255 - 510 * (1 - occ);
316  if (coeft < 0)
317  {
318  coeft = 0;
319  }
320  vx_color = TColor(
321  coefc * general_color.R,
322  coefc * general_color.G,
323  coefc * general_color.B, coeft);
324  break;
325 
326  default:
327  THROW_EXCEPTION("Unknown coloring scheme!");
328  }
329 
330  const size_t vx_set =
331  is_occupied ? VOXEL_SET_OCCUPIED : VOXEL_SET_FREESPACE;
332 
333  gl_obj.push_back_Voxel(
334  vx_set,
336  mrpt::math::TPoint3D(x, y, z), 2 * L, vx_color));
337  }
338 
340  {
341  // Not leaf-nodes:
342  const mrpt::math::TPoint3D pt_min(x - L, y - L, z - L);
343  const mrpt::math::TPoint3D pt_max(x + L, y + L, z + L);
344  gl_obj.push_back_GridCube(
345  COctoMapVoxels::TGridCube(pt_min, pt_max));
346  }
347  }
348  }
349  } // end for each voxel
350 
351  // if we use transparency, sort cubes by "Z" as an approximation to
352  // far-to-near render ordering:
353  if (gl_obj.isCubeTransparencyEnabled()) gl_obj.sort_voxels_by_z();
354 
355  // Set bounding box:
356  gl_obj.setBoundingBox(bbmin, bbmax);
357 
358  MRPT_END
359 }
360 
362  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
363 {
364  auto gl_obj = mrpt::opengl::COctoMapVoxels::Create();
365  this->getAsOctoMapVoxels(*gl_obj);
366  outObj->insert(gl_obj);
367 }
368 
369 uint8_t COccupancyGridMap3D::serializeGetVersion() const { return 0; }
371 {
372 // Version 2: Save OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS/16BITS
373 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
374  out << uint8_t(8);
375 #else
376  out << uint8_t(16);
377 #endif
378 
379  // Save grid dimensions:
381 
382 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
383  out.WriteBuffer
384 #else
385  out.WriteBufferFixEndianness
386 #endif
387  (m_grid.cellByIndex(0, 0, 0), sizeof(cell_t) * m_grid.getSizeX() *
388  m_grid.getSizeY() *
389  m_grid.getSizeZ());
390 
391  // insertionOptions:
396 
397  // Likelihood:
406 
408 
410 }
411 
413  mrpt::serialization::CArchive& in, uint8_t version)
414 {
415  m_is_empty = false;
416 
417  switch (version)
418  {
419  case 0:
420  {
421  uint8_t bitsPerCellStream;
422  in >> bitsPerCellStream;
423 
424 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
425  ASSERT_(bitsPerCellStream == 8);
426 #else
427  ASSERT_(bitsPerCellStream == 16);
428 #endif
429 
430  // Save grid dimensions:
432 
433 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
434  in.ReadBuffer
435 #else
436  in.ReadBufferFixEndianness
437 #endif
438  (m_grid.cellByIndex(0, 0, 0),
439  sizeof(cell_t) * m_grid.getSizeX() * m_grid.getSizeY() *
440  m_grid.getSizeZ());
441 
442  // insertionOptions:
448 
449  // Likelihood:
450  in.ReadAsAndCastTo<int32_t, TLikelihoodMethod>(
452 
460 
461  in >> genericMapParams;
462 
464  }
465  break;
466  default:
468  };
469 }
470 
473 {
474  const int8_t version = 0;
475  out << version;
476  out << generateGridLines << generateOccupiedVoxels << visibleOccupiedVoxels
477  << generateFreeVoxels << visibleFreeVoxels;
478 }
479 
482 {
483  int8_t version;
484  in >> version;
485  switch (version)
486  {
487  case 0:
488  {
489  in >> generateGridLines >> generateOccupiedVoxels >>
490  visibleOccupiedVoxels >> generateFreeVoxels >>
491  visibleFreeVoxels;
492  }
493  break;
494  default:
496  }
497 }
498 
500  const std::string& filNamePrefix) const
501 {
502  using namespace std::string_literals;
503  const auto fil = filNamePrefix + ".txt";
504  // todo
505 }
void showVoxels(unsigned int voxel_set, bool show)
Shows/hides the voxels (voxel_set is a 0-based index for the set of voxels to modify, e.g.
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Parameters for CMetricMap::compute3DMatchingRatio()
void reserveVoxels(const size_t set_index, const size_t nVoxels)
#define MRPT_START
Definition: exceptions.h:241
GLdouble GLdouble z
Definition: glext.h:3879
double x
X,Y,Z coordinates.
Definition: TPoint3D.h:83
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation ...
void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
See docs in base class: in this class this always returns 0.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
static CLogOddsGridMapLUT< COccupancyGridMap3D::voxelType > logodd_lut
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void dyngridcommon_writeToStream(ARCHIVE &out) const
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
void fill(const T &value)
Fills all the cells with the same value.
float getCellFreeness(unsigned int cx, unsigned int cy, unsigned int cz) const
Read the real valued [0,1] (0:occupied, 1:free) contents of a voxel, given its index.
static CLogOddsGridMapLUT< voxelType > & get_logodd_lut()
Lookup tables for log-odds.
uint16_t decimation
Decimation for insertPointCloud() or 2D range scans (Default: 1)
void setBoundingBox(const mrpt::math::TPoint3D &bb_min, const mrpt::math::TPoint3D &bb_max)
Manually changes the bounding box (normally the user doesn&#39;t need to call this)
void reserveGridCubes(const size_t nCubes)
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0.35
coord_t idx2x(int cx) const
Transform a voxel index into a coordinate value of the voxel central point.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
void clear()
Clears everything.
mrpt::maps::COccupancyGridMap3D::TInsertionOptions insertionOpts
Observations insertion options.
uint16_t decimation_3d_range
Specify the decimation of 3D range scans.
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
GLdouble s
Definition: glext.h:3682
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2) ...
mrpt::maps::TMapGenericParams genericMapParams
Common params for all maps: These are automatically set in TMetricMapTypesRegistry::factoryMapObjectF...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void push_back_GridCube(const TGridCube &c)
This class allows loading and storing values and vectors of different types from a configuration text...
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
OccGridCellTraits::cellType cell_t
The type of cells.
virtual void resize(coord_t new_x_min, coord_t new_x_max, coord_t new_y_min, coord_t new_y_max, coord_t new_z_min, coord_t new_z_max, const T &defaultValueNewCells, coord_t additionalMarginMeters=2)
Changes the size of the grid, maintaining previous contents.
bool isOutOfBounds(const int cx, const int cy, const int cz) const
void updateCell(int cx_idx, int cy_idx, int cz_idx, float v)
Performs the Bayesian fusion of a new observation of a cell.
float maxDistanceInsertion
Largest distance at which voxels are updated (Default: 15 meters)
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
A list of TMatchingPair.
Definition: TMatchingPair.h:70
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true) ...
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 this always returns 0.
void internal_clear() override
Clear the map: It set all voxels to their default occupancy value (0.5), without changing the resolut...
constexpr auto sect
T * cellByIndex(unsigned int cx, unsigned int cy, unsigned int cz)
Returns a pointer to the contents of a voxel given by its voxel indexes, or nullptr if it is out of t...
bool m_is_empty
True upon construction; used by isEmpty()
void resizeGrid(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, float new_voxels_default_value=0.5f)
Change the size of gridmap, maintaining previous contents.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
virtual void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
__int32 int32_t
Definition: glext.h:3455
static Ptr Create(Args &&... args)
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
GLsizei const GLchar ** string
Definition: glext.h:4116
void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
The info of each grid block.
void push_back_Voxel(const size_t set_index, const TVoxel &v)
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
float maxFreenessUpdateCertainty
A value in the range [0.5,1] for updating a free voxel.
A 3D occupancy grid map with a regular, even distribution of voxels.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#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...
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
The info of each of the voxels.
const GLdouble * v
Definition: glext.h:3684
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.
TLikelihoodMethod
The type for selecting a likelihood computation method.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
void resizeVoxelSets(const size_t nVoxelSets)
void fill(float default_value=0.5f)
Fills all the voxels with a default value.
bool isEmpty() const override
Returns true upon map construction or after calling clear(), the return changes to false upon success...
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating voxel with a Bayesian approach (default 0...
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree, but computationally costly (Default: false)
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
mrpt::img::TColorf getColor() const
Returns the object color property as a TColorf.
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
visualization_mode_t getVisualizationMode() const
GLuint in
Definition: glext.h:7391
COccupancyGridMap3D(const mrpt::math::TPoint3D &corner_min={-5.0f, -5.0f, -5.0f}, const mrpt::math::TPoint3D &corner_max={5.0f, 5.0f, 5.0f}, float resolution=0.25f)
Constructor.
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
float min_x
See COccupancyGridMap3D::COccupancyGridMap3D.
virtual void setSize(const coord_t x_min, const coord_t x_max, const coord_t y_min, const coord_t y_max, const coord_t z_min, const coord_t z_max, const coord_t resolution_xy, const coord_t resolution_z_=-1.0, const T *fill_value=nullptr)
Changes the size of the grid, ERASING all previous contents.
GLenum GLint GLint y
Definition: glext.h:3542
A generic provider of log-odds grid-map maintainance functions.
static voxelType p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
void saveMetricMapRepresentationToFile(const std::string &f) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:161
GLuint res
Definition: glext.h:7385
A RGB color - 8bit.
Definition: TColor.h:20
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
Definition: TPoint3D.h:90
float rayTracing_stdHit
[rayTracing] The laser range sigma.
Parameters for the determination of matchings between point clouds, etc.
void setSize(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, float resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
bool isCubeTransparencyEnabled() const
GLenum const GLfloat * params
Definition: glext.h:3538
void dyngridcommon_readFromStream(ARCHIVE &in)
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
OccGridCellTraits::cellType voxelType
The type of the map voxels:
void dumpToTextStream_map_specific(std::ostream &out) const override
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)
#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: cb8dd5fc8 Sat Dec 7 21:55:39 2019 +0100 at sáb dic 7 22:00:13 CET 2019