MRPT  2.0.4
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-2020, 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 
29  const mrpt::config::CConfigFileBase& source, const std::string& sect)
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  double res, float default_value)
101 {
102  MRPT_START
103 
104  ASSERT_ABOVE_(res, 0.0);
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  [[maybe_unused]] const mrpt::maps::CMetricMap* otherMap,
207  [[maybe_unused]] const mrpt::poses::CPose3D& otherMapPose,
208  [[maybe_unused]] const TMatchingRatioParams& params) const
209 {
210  THROW_EXCEPTION("Implement me!");
211  return 0;
212 }
213 
215  mrpt::opengl::COctoMapVoxels& gl_obj) const
216 {
217  MRPT_START
218 
219  using mrpt::img::TColor;
220  using mrpt::img::TColorf;
221  using namespace mrpt::opengl;
222 
223  const size_t N = m_grid.getSizeX() * m_grid.getSizeY() * m_grid.getSizeZ();
224  const TColorf general_color = gl_obj.getColor();
225  const TColor general_color_u = general_color.asTColor();
226 
227  gl_obj.clear();
228  gl_obj.resizeVoxelSets(2); // 2 sets of voxels: occupied & free
230 
231  gl_obj.showVoxels(
234  gl_obj.showVoxels(
236 
239 
240  const mrpt::math::TPoint3D bbmin(
242  const mrpt::math::TPoint3D bbmax(
244  const float inv_dz = 1.0f / d2f(bbmax.z - bbmin.z + 0.01f);
245  const double L = 0.5 * m_grid.getResolutionZ();
246 
247  for (size_t cz = 0; cz < m_grid.getSizeZ(); cz++)
248  {
249  // voxel center coordinates:
250  const double z = m_grid.idx2z(cz) + m_grid.getResolutionZ() * 0.5;
251  for (size_t cy = 0; cy < m_grid.getSizeY(); cy++)
252  {
253  const double y = m_grid.idx2y(cy) + m_grid.getResolutionXY() * 0.5;
254  for (size_t cx = 0; cx < m_grid.getSizeX(); cx++)
255  {
256  const double x =
257  m_grid.idx2x(cx) + m_grid.getResolutionXY() * 0.5;
258  const float occ = 1.0f - this->getCellFreeness(cx, cy, cz);
259  const bool is_occupied = occ > 0.501f;
260  const bool is_free = occ < 0.499f;
261  if ((is_occupied && renderingOptions.generateOccupiedVoxels) ||
263  {
264  mrpt::img::TColor vx_color;
265  float coefc, coeft;
266  switch (gl_obj.getVisualizationMode())
267  {
268  case COctoMapVoxels::FIXED:
269  vx_color = general_color_u;
270  break;
271  case COctoMapVoxels::COLOR_FROM_HEIGHT:
272  coefc = 255 * inv_dz * d2f(z - bbmin.z);
273  vx_color = TColor(
274  f2u8(coefc * general_color.R),
275  f2u8(coefc * general_color.G),
276  f2u8(coefc * general_color.B),
277  f2u8(255 * general_color.A));
278  break;
279 
280  case COctoMapVoxels::COLOR_FROM_OCCUPANCY:
281  coefc = 240 * (1 - occ) + 15;
282  vx_color = TColor(
283  f2u8(coefc * general_color.R),
284  f2u8(coefc * general_color.G),
285  f2u8(coefc * general_color.B),
286  f2u8(255 * general_color.A));
287  break;
288 
289  case COctoMapVoxels::TRANSPARENCY_FROM_OCCUPANCY:
290  coeft = 255 - 510 * (1 - occ);
291  if (coeft < 0)
292  {
293  coeft = 0;
294  }
295  vx_color = general_color.asTColor();
296  vx_color.A = mrpt::round(coeft);
297  break;
298 
299  case COctoMapVoxels::TRANS_AND_COLOR_FROM_OCCUPANCY:
300  coefc = 240 * (1 - occ) + 15;
301  vx_color = TColor(
302  f2u8(coefc * general_color.R),
303  f2u8(coefc * general_color.G),
304  f2u8(coefc * general_color.B), 50);
305  break;
306 
307  case COctoMapVoxels::MIXED:
308  coefc = d2f(255 * inv_dz * (z - bbmin.z));
309  coeft = d2f(255 - 510 * (1 - occ));
310  if (coeft < 0)
311  {
312  coeft = 0;
313  }
314  vx_color = TColor(
315  f2u8(coefc * general_color.R),
316  f2u8(coefc * general_color.G),
317  f2u8(coefc * general_color.B),
318  static_cast<uint8_t>(coeft));
319  break;
320 
321  default:
322  THROW_EXCEPTION("Unknown coloring scheme!");
323  }
324 
325  const size_t vx_set =
326  is_occupied ? VOXEL_SET_OCCUPIED : VOXEL_SET_FREESPACE;
327 
328  gl_obj.push_back_Voxel(
329  vx_set,
331  mrpt::math::TPoint3D(x, y, z), 2 * L, vx_color));
332  }
333 
335  {
336  // Not leaf-nodes:
337  const mrpt::math::TPoint3D pt_min(x - L, y - L, z - L);
338  const mrpt::math::TPoint3D pt_max(x + L, y + L, z + L);
339  gl_obj.push_back_GridCube(
340  COctoMapVoxels::TGridCube(pt_min, pt_max));
341  }
342  }
343  }
344  } // end for each voxel
345 
346  // if we use transparency, sort cubes by "Z" as an approximation to
347  // far-to-near render ordering:
348  if (gl_obj.isCubeTransparencyEnabled()) gl_obj.sort_voxels_by_z();
349 
350  // Set bounding box:
351  gl_obj.setBoundingBox(bbmin, bbmax);
352 
353  MRPT_END
354 }
355 
357  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
358 {
359  auto gl_obj = mrpt::opengl::COctoMapVoxels::Create();
360  this->getAsOctoMapVoxels(*gl_obj);
361  outObj->insert(gl_obj);
362 }
363 
364 uint8_t COccupancyGridMap3D::serializeGetVersion() const { return 0; }
366 {
367 // Version 2: Save OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS/16BITS
368 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
369  out << uint8_t(8);
370 #else
371  out << uint8_t(16);
372 #endif
373 
374  // Save grid dimensions:
376 
377 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
378  out.WriteBuffer
379 #else
380  out.WriteBufferFixEndianness
381 #endif
382  (m_grid.cellByIndex(0, 0, 0), sizeof(cell_t) * m_grid.getSizeX() *
383  m_grid.getSizeY() *
384  m_grid.getSizeZ());
385 
386  // insertionOptions:
391 
392  // Likelihood:
393  out.WriteAs<int32_t>(likelihoodOptions.likelihoodMethod);
401 
403 
405 }
406 
408  mrpt::serialization::CArchive& in, uint8_t version)
409 {
410  m_is_empty = false;
411 
412  switch (version)
413  {
414  case 0:
415  {
416  uint8_t bitsPerCellStream;
417  in >> bitsPerCellStream;
418 
419 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
420  ASSERT_(bitsPerCellStream == 8);
421 #else
422  ASSERT_(bitsPerCellStream == 16);
423 #endif
424 
425  // Save grid dimensions:
427 
428 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
429  in.ReadBuffer
430 #else
432 #endif
433  (m_grid.cellByIndex(0, 0, 0),
434  sizeof(cell_t) * m_grid.getSizeX() * m_grid.getSizeY() *
435  m_grid.getSizeZ());
436 
437  // insertionOptions:
443 
444  // Likelihood:
445  in.ReadAsAndCastTo<int32_t, TLikelihoodMethod>(
447 
455 
456  in >> genericMapParams;
457 
459  }
460  break;
461  default:
463  };
464 }
465 
468 {
469  const int8_t version = 0;
470  out << version;
471  out << generateGridLines << generateOccupiedVoxels << visibleOccupiedVoxels
472  << generateFreeVoxels << visibleFreeVoxels;
473 }
474 
477 {
478  int8_t version;
479  in >> version;
480  switch (version)
481  {
482  case 0:
483  {
484  in >> generateGridLines >> generateOccupiedVoxels >>
485  visibleOccupiedVoxels >> generateFreeVoxels >>
486  visibleFreeVoxels;
487  }
488  break;
489  default:
491  }
492 }
493 
495  const std::string& filNamePrefix) const
496 {
497  using namespace std::string_literals;
498  const auto fil = filNamePrefix + ".txt";
499  // todo
500 }
mrpt::maps::COccupancyGridMap3D::updateCell
void updateCell(int cx_idx, int cy_idx, int cz_idx, float v)
Performs the Bayesian fusion of a new observation of a cell.
Definition: COccupancyGridMap3D.cpp:159
mrpt::opengl::COctoMapVoxels::isCubeTransparencyEnabled
bool isCubeTransparencyEnabled() const
Definition: COctoMapVoxels.h:205
mrpt::maps::COccupancyGridMap3D::serializeGetVersion
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: COccupancyGridMap3D.cpp:364
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
Definition: COccupancyGridMap3D_likelihood.cpp:37
mrpt::maps::COccupancyGridMap3D::TRenderingOptions::generateFreeVoxels
bool generateFreeVoxels
Generate voxels for the freespace (Default=true)
Definition: COccupancyGridMap3D.h:323
ASSERT_ABOVE_
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
mrpt::maps::COccupancyGridMap3D::resizeGrid
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.
Definition: COccupancyGridMap3D.cpp:121
mrpt::maps::COccupancyGridMap3D::m_is_empty
bool m_is_empty
True upon construction; used by isEmpty()
Definition: COccupancyGridMap3D.h:48
mrpt::maps::COccupancyGridMap3D::serializeTo
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: COccupancyGridMap3D.cpp:365
mrpt::containers::CDynamicGrid3D::cellByIndex
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...
Definition: CDynamicGrid3D.h:285
mrpt::maps::COccupancyGridMap3D::serializeFrom
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: COccupancyGridMap3D.cpp:407
mrpt::maps::COccupancyGridMap3D::fill
void fill(float default_value=0.5f)
Fills all the voxels with a default value.
Definition: COccupancyGridMap3D.cpp:152
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_maxCorrsDistance
float LF_maxCorrsDistance
[LikelihoodField] The max.
Definition: COccupancyGridMap3D.h:290
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::rayTracing_stdHit
float rayTracing_stdHit
[rayTracing] The laser range sigma.
Definition: COccupancyGridMap3D.h:299
mrpt::maps::COccupancyGridMap3D::TLikelihoodMethod
TLikelihoodMethod
The type for selecting a likelihood computation method.
Definition: COccupancyGridMap3D.h:243
mrpt::opengl::CSetOfObjects::Ptr
std::shared_ptr< mrpt::opengl ::CSetOfObjects > Ptr
Definition: CSetOfObjects.h:28
COctoMapVoxels.h
mrpt::math::TPoint3D_< double >
mrpt::maps::CLogOddsGridMap3D< OccGridCellTraits::cellType >::m_grid
grid_t m_grid
The actual 3D voxels container.
Definition: CLogOddsGridMap3D.h:35
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:306
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_maxRange
float LF_maxRange
[LikelihoodField] The max.
Definition: COccupancyGridMap3D.h:284
mrpt::containers::CDynamicGrid3D::getResolutionZ
coord_t getResolutionZ() const
Definition: CDynamicGrid3D.h:325
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::loadFromConfigFile
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
Definition: COccupancyGridMap3D_insert.cpp:211
mrpt::containers::CDynamicGrid3D::isOutOfBounds
bool isOutOfBounds(const int cx, const int cy, const int cz) const
Definition: CDynamicGrid3D.h:230
mrpt::math::TPoint3D_data::z
T z
Definition: TPoint3D.h:29
mrpt::containers::CDynamicGrid3D::getSizeX
size_t getSizeX() const
Definition: CDynamicGrid3D.h:314
mrpt::maps::COccupancyGridMap3D::getAsOctoMapVoxels
void getAsOctoMapVoxels(mrpt::opengl::COctoMapVoxels &gl_obj) const
Definition: COccupancyGridMap3D.cpp:214
mrpt::opengl::COctoMapVoxels::TGridCube
The info of each grid block.
Definition: COctoMapVoxels.h:115
mrpt::maps::COccupancyGridMap3D::TMapDefinition::resolution
float resolution
Definition: COccupancyGridMap3D.h:393
MAP_DEFINITION_REGISTER
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Definition: TMetricMapTypesRegistry.h:91
mrpt::containers::CDynamicGrid3D::getXMax
coord_t getXMax() const
Definition: CDynamicGrid3D.h:319
mrpt::maps::COccupancyGridMap3D::TMapDefinition::insertionOpts
mrpt::maps::COccupancyGridMap3D::TInsertionOptions insertionOpts
Observations insertion options.
Definition: COccupancyGridMap3D.h:396
mrpt::f2u8
uint8_t f2u8(const float f)
converts a float [0,1] into an uint8_t [0,255] (without checking for out of bounds)
Definition: core/include/mrpt/core/bits_math.h:193
mrpt::maps::COccupancyGridMap3D::TMapDefinition::max_z
float max_z
Definition: COccupancyGridMap3D.h:392
mrpt::opengl::COctoMapVoxels::resizeVoxelSets
void resizeVoxelSets(const size_t nVoxelSets)
Definition: COctoMapVoxels.h:289
mrpt::containers::CDynamicGrid3D::getZMin
coord_t getZMin() const
Definition: CDynamicGrid3D.h:322
sect
constexpr auto sect
Definition: ICP_SLAM_App.cpp:33
mrpt::img::TColorf::asTColor
TColor asTColor() const
Returns the 0-255 integer version of this color: RGBA_u8
Definition: TColor.h:101
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:64
mrpt::containers::CDynamicGrid3D::idx2z
coord_t idx2z(int cz) const
Definition: CDynamicGrid3D.h:350
mrpt::maps::COccupancyGridMap3D::isEmpty
bool isEmpty() const override
Returns true upon map construction or after calling clear(), the return changes to false upon success...
Definition: COccupancyGridMap3D.cpp:203
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::maps::COccupancyGridMap3D::COccupancyGridMap3D
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.
Definition: COccupancyGridMap3D.cpp:89
logodd_lut
static CLogOddsGridMapLUT< COccupancyGridMap3D::voxelType > logodd_lut
Definition: COccupancyGridMap3D.cpp:81
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::serialization::CArchive::ReadBuffer
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
mrpt::maps::TMetricMapInitializer
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Definition: TMetricMapInitializer.h:32
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::decimation_3d_range
uint16_t decimation_3d_range
Specify the decimation of 3D range scans.
Definition: COccupancyGridMap3D.h:232
mrpt::maps::COccupancyGridMap3D::TRenderingOptions::writeToStream
void writeToStream(mrpt::serialization::CArchive &out) const
Binary dump to stream.
Definition: COccupancyGridMap3D.cpp:466
mrpt::maps::COccupancyGridMap3D::TRenderingOptions::readFromStream
void readFromStream(mrpt::serialization::CArchive &in)
Binary dump to stream.
Definition: COccupancyGridMap3D.cpp:475
mrpt::maps::TMetricMapInitializer::genericMapParams
mrpt::maps::TMapGenericParams genericMapParams
Common params for all maps: These are automatically set in TMetricMapTypesRegistry::factoryMapObjectF...
Definition: TMetricMapInitializer.h:40
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::maps::COccupancyGridMap3D::p2l
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...
Definition: COccupancyGridMap3D.h:122
mrpt::maps::COccupancyGridMap3D::saveMetricMapRepresentationToFile
void saveMetricMapRepresentationToFile(const std::string &f) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
Definition: COccupancyGridMap3D.cpp:494
mrpt::serialization::CArchive
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::opengl::COctoMapVoxels::reserveVoxels
void reserveVoxels(const size_t set_index, const size_t nVoxels)
Definition: COctoMapVoxels.h:305
mrpt::maps::COccupancyGridMap3D::TMapDefinition::loadFromConfigFile_map_specific
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
Definition: COccupancyGridMap3D.cpp:28
mrpt::maps::CLogOddsGridMap3D
A generic provider of log-odds grid-map maintainance functions.
Definition: CLogOddsGridMap3D.h:27
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_useSquareDist
bool LF_useSquareDist
[LikelihoodField] (Default:false) Use exp(dist^2/std^2) instead of exp(dist^2/std^2)
Definition: COccupancyGridMap3D.h:293
mrpt::maps::CLogOddsGridMapLUT
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
Definition: CLogOddsGridMapLUT.h:27
mrpt::opengl::COctoMapVoxels::clear
void clear()
Clears everything.
Definition: COctoMapVoxels.cpp:27
mrpt::maps::COccupancyGridMap3D::TMapDefinition::TMapDefinition
TMapDefinition()
CConfigFileBase.h
mrpt::maps::COccupancyGridMap3D::determineMatching2D
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.
Definition: COccupancyGridMap3D.cpp:189
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
mrpt::opengl::COctoMapVoxels::push_back_GridCube
void push_back_GridCube(const TGridCube &c)
Definition: COctoMapVoxels.h:342
mrpt::maps::COccupancyGridMap3D::voxelType
OccGridCellTraits::cellType voxelType
The type of the map voxels:
Definition: COccupancyGridMap3D.h:40
ASSERT_BELOWEQ_
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:161
mrpt::opengl::COctoMapVoxels::sort_voxels_by_z
void sort_voxels_by_z()
Definition: COctoMapVoxels.cpp:321
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_zHit
float LF_zHit
[LikelihoodField]
Definition: COccupancyGridMap3D.h:282
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::maxDistanceInsertion
float maxDistanceInsertion
Largest distance at which voxels are updated (Default: 15 meters)
Definition: COccupancyGridMap3D.h:220
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
mrpt::opengl::COctoMapVoxels::setBoundingBox
void setBoundingBox(const mrpt::math::TPoint3D &bb_min, const mrpt::math::TPoint3D &bb_max)
Manually changes the bounding box (normally the user doesn't need to call this)
Definition: COctoMapVoxels.cpp:35
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::maps::COccupancyGridMap3D::getCellFreeness
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.
Definition: COccupancyGridMap3D.h:142
mrpt::containers::CDynamicGrid3D::fill
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid3D.h:223
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::containers::CDynamicGrid3D::getSizeY
size_t getSizeY() const
Definition: CDynamicGrid3D.h:315
mrpt::maps::COccupancyGridMap3D::get_logodd_lut
static CLogOddsGridMapLUT< voxelType > & get_logodd_lut()
Lookup tables for log-odds.
Definition: COccupancyGridMap3D.cpp:84
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::maxFreenessUpdateCertainty
float maxFreenessUpdateCertainty
A value in the range [0.5,1] for updating a free voxel.
Definition: COccupancyGridMap3D.h:226
mrpt::containers::CDynamicGrid3D::getXMin
coord_t getXMin() const
Definition: CDynamicGrid3D.h:318
mrpt::img::TColorf
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
mrpt::maps::COccupancyGridMap3D::TMapDefinition::likelihoodOpts
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: COccupancyGridMap3D.h:398
mrpt::maps::COccupancyGridMap3D::likelihoodOptions
TLikelihoodOptions likelihoodOptions
Definition: COccupancyGridMap3D.h:302
mrpt::opengl::COctoMapVoxels::showVoxels
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,...
Definition: COctoMapVoxels.h:219
mrpt::img::TColor
A RGB color - 8bit.
Definition: TColor.h:25
mrpt::maps::COccupancyGridMap3D::TRenderingOptions::visibleOccupiedVoxels
bool visibleOccupiedVoxels
Set occupied voxels visible (requires generateOccupiedVoxels=true) (Default=true)
Definition: COccupancyGridMap3D.h:320
mrpt::d2f
float d2f(const double d)
shortcut for static_cast<float>(double)
Definition: core/include/mrpt/core/bits_math.h:189
mrpt::maps::COccupancyGridMap3D::renderingOptions
TRenderingOptions renderingOptions
Definition: COccupancyGridMap3D.h:335
mrpt::maps::COccupancyGridMap3D::TRenderingOptions::generateGridLines
bool generateGridLines
Generate grid lines for all octree nodes, useful to draw the "structure" of the octree,...
Definition: COccupancyGridMap3D.h:313
mrpt::maps::COccupancyGridMap3D::TMapDefinition
Definition: COccupancyGridMap3D.h:388
mrpt::maps::COccupancyGridMap3D::TMapDefinition::min_x
float min_x
See COccupancyGridMap3D::COccupancyGridMap3D.
Definition: COccupancyGridMap3D.h:390
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_stdHit
float LF_stdHit
[LikelihoodField] The laser range "sigma" used in computations; Default value = 0....
Definition: COccupancyGridMap3D.h:280
mrpt::opengl::VOXEL_SET_OCCUPIED
@ VOXEL_SET_OCCUPIED
Definition: COctoMapVoxels.h:20
LOADABLEOPTS_DUMP_VAR
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
Definition: config/CLoadableOptions.h:101
mrpt::maps::COccupancyGridMap3D::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
Definition: COccupancyGridMap3D.cpp:356
mrpt::img::TColor::A
uint8_t A
Definition: TColor.h:51
mrpt::maps::COccupancyGridMap3D::TRenderingOptions::generateOccupiedVoxels
bool generateOccupiedVoxels
Generate voxels for the occupied volumes (Default=true)
Definition: COccupancyGridMap3D.h:316
mrpt::containers::CDynamicGrid3D::dyngridcommon_readFromStream
void dyngridcommon_readFromStream(ARCHIVE &in)
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
Definition: CDynamicGrid3D.h:382
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::opengl::VOXEL_SET_FREESPACE
@ VOXEL_SET_FREESPACE
Definition: COctoMapVoxels.h:21
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::likelihoodMethod
TLikelihoodMethod likelihoodMethod
The selected method to compute an observation likelihood.
Definition: COccupancyGridMap3D.h:277
mrpt::opengl::COctoMapVoxels::reserveGridCubes
void reserveGridCubes(const size_t nCubes)
Definition: COctoMapVoxels.h:301
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:20
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::rayTracing_decimation
int32_t rayTracing_decimation
[rayTracing] One out of "rayTracing_decimation" rays will be simulated and compared only: set to 1 to...
Definition: COccupancyGridMap3D.h:297
mrpt::maps::COccupancyGridMap3D::setSize
void setSize(const mrpt::math::TPoint3D &corner_min, const mrpt::math::TPoint3D &corner_max, double resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
Definition: COccupancyGridMap3D.cpp:98
mrpt::containers::CDynamicGrid3D::getSizeZ
size_t getSizeZ() const
Definition: CDynamicGrid3D.h:316
mrpt::containers::CDynamicGrid3D::getZMax
coord_t getZMax() const
Definition: CDynamicGrid3D.h:323
mrpt::math::TPoint3D_data::y
T y
Definition: TPoint3D.h:29
mrpt::containers::CDynamicGrid3D::getYMax
coord_t getYMax() const
Definition: CDynamicGrid3D.h:321
mrpt::containers::CDynamicGrid3D::getResolutionXY
coord_t getResolutionXY() const
Definition: CDynamicGrid3D.h:324
mrpt::opengl::COctoMapVoxels
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
Definition: COctoMapVoxels.h:69
mrpt::serialization::CArchive::ReadAsAndCastTo
void ReadAsAndCastTo(CAST_TO_TYPE &read_here)
Read a value from a stream stored in a type different of the target variable, making the conversion v...
Definition: CArchive.h:147
CPose3D.h
ASSERT_ABOVEEQ_
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
mrpt::maps::COccupancyGridMap3D::TRenderingOptions::visibleFreeVoxels
bool visibleFreeVoxels
Set free voxels visible (requires generateFreeVoxels=true) (Default=true)
Definition: COccupancyGridMap3D.h:327
mrpt::maps::COccupancyGridMap3D::insertionOptions
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
Definition: COccupancyGridMap3D.h:240
mrpt::opengl::COctoMapVoxels::Create
static Ptr Create(Args &&... args)
Definition: COctoMapVoxels.h:73
mrpt::serialization::CArchive::ReadBufferFixEndianness
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
maps-precomp.h
mrpt::opengl::COctoMapVoxels::push_back_Voxel
void push_back_Voxel(const size_t set_index, const TVoxel &v)
Definition: COctoMapVoxels.h:347
mrpt::containers::CDynamicGrid3D::resize
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.
Definition: CDynamicGrid3D.h:48
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::opengl::CRenderizable::getColor
mrpt::img::TColorf getColor() const
Returns the object color property as a TColorf.
Definition: CRenderizable.h:229
mrpt::math::TPoint3D_data::x
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
mrpt::maps::COccupancyGridMap3D
A 3D occupancy grid map with a regular, even distribution of voxels.
Definition: COccupancyGridMap3D.h:33
mrpt::maps::COccupancyGridMap3D::TMapDefinition::min_z
float min_z
Definition: COccupancyGridMap3D.h:392
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::decimation
uint16_t decimation
Decimation for insertPointCloud() or 2D range scans (Default: 1)
Definition: COccupancyGridMap3D.h:235
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_zRandom
float LF_zRandom
Definition: COccupancyGridMap3D.h:282
mrpt::containers::CDynamicGrid3D::idx2y
coord_t idx2y(int cy) const
Definition: CDynamicGrid3D.h:346
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
mrpt::maps::CLogOddsGridMap3D< OccGridCellTraits::cellType >::cell_t
OccGridCellTraits::cellType cell_t
The type of cells.
Definition: CLogOddsGridMap3D.h:30
mrpt::maps
Definition: CBeacon.h:21
mrpt::maps::TMatchingExtraResults
Additional results from the determination of matchings between point clouds, etc.,...
Definition: metric_map_types.h:51
CArchive.h
MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CSimplePointsMap.h
mrpt::containers::CDynamicGrid3D::idx2x
coord_t idx2x(int cx) const
Transform a voxel index into a coordinate value of the voxel central point.
Definition: CDynamicGrid3D.h:342
mrpt::maps::COccupancyGridMap3D::TMapDefinition::max_x
float max_x
Definition: COccupancyGridMap3D.h:390
mrpt::maps::COccupancyGridMap3D::TMapDefinition::min_y
float min_y
Definition: COccupancyGridMap3D.h:391
COccupancyGridMap3D.h
mrpt::containers::CDynamicGrid3D::getYMin
coord_t getYMin() const
Definition: CDynamicGrid3D.h:320
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::containers::CDynamicGrid3D::setSize
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.
Definition: CDynamicGrid3D.h:175
mrpt::maps::COccupancyGridMap3D::TInsertionOptions::maxOccupancyUpdateCertainty
float maxOccupancyUpdateCertainty
A value in the range [0.5,1] used for updating voxel with a Bayesian approach (default 0....
Definition: COccupancyGridMap3D.h:223
mrpt::maps::COccupancyGridMap3D::compute3DMatchingRatio
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.
Definition: COccupancyGridMap3D.cpp:205
mrpt::config::CLoadableOptions::dumpToTextStream
virtual void dumpToTextStream(std::ostream &out) const
This method should clearly display all the contents of the structure in textual form,...
Definition: CLoadableOptions.cpp:76
mrpt::maps::COccupancyGridMap3D::internal_clear
void internal_clear() override
Clear the map: It set all voxels to their default occupancy value (0.5), without changing the resolut...
Definition: COccupancyGridMap3D.cpp:139
mrpt::maps::COccupancyGridMap3D::TMapDefinition::dumpToTextStream_map_specific
void dumpToTextStream_map_specific(std::ostream &out) const override
Definition: COccupancyGridMap3D.cpp:50
mrpt::maps::COccupancyGridMap3D::TMapDefinition::max_y
float max_y
Definition: COccupancyGridMap3D.h:391
mrpt::opengl::COctoMapVoxels::TVoxel
The info of each of the voxels.
Definition: COctoMapVoxels.h:99
mrpt::maps::COccupancyGridMap3D::TLikelihoodOptions::LF_decimation
uint32_t LF_decimation
[LikelihoodField] The decimation of the points in a scan, default=1 == no decimation
Definition: COccupancyGridMap3D.h:287
mrpt::maps::COccupancyGridMap3D::internal_CreateFromMapDefinition
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
Definition: COccupancyGridMap3D.cpp:65
mrpt::containers::CDynamicGrid3D::dyngridcommon_writeToStream
void dyngridcommon_writeToStream(ARCHIVE &out) const
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
Definition: CDynamicGrid3D.h:371
mrpt::opengl::COctoMapVoxels::getVisualizationMode
visualization_mode_t getVisualizationMode() const
Definition: COctoMapVoxels.h:186



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020