MRPT  1.9.9
COccupancyGridMap2D_common.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 
12 // Force size_x being a multiple of 16 cells
13 //#define ROWSIZE_MULTIPLE_16
14 
17 #include <mrpt/math/TPose2D.h>
18 #include <mrpt/poses/CPose3D.h>
20 
21 using namespace mrpt;
22 using namespace mrpt::math;
23 using namespace mrpt::maps;
24 using namespace mrpt::obs;
25 using namespace mrpt::poses;
26 using namespace mrpt::tfest;
27 using namespace std;
28 
29 // =========== Begin of Map definition ============
31  "mrpt::maps::COccupancyGridMap2D,occupancyGrid",
33 
34 COccupancyGridMap2D::TMapDefinition::TMapDefinition() = default;
35 
36 void COccupancyGridMap2D::TMapDefinition::loadFromConfigFile_map_specific(
37  const mrpt::config::CConfigFileBase& source,
38  const std::string& sectionNamePrefix)
39 {
40  // [<sectionNamePrefix>+"_creationOpts"]
41  const std::string sSectCreation =
42  sectionNamePrefix + string("_creationOpts");
43  MRPT_LOAD_CONFIG_VAR(min_x, float, source, sSectCreation);
44  MRPT_LOAD_CONFIG_VAR(max_x, float, source, sSectCreation);
45  MRPT_LOAD_CONFIG_VAR(min_y, float, source, sSectCreation);
46  MRPT_LOAD_CONFIG_VAR(max_y, float, source, sSectCreation);
47  MRPT_LOAD_CONFIG_VAR(resolution, float, source, sSectCreation);
48 
49  // [<sectionName>+"_occupancyGrid_##_insertOpts"]
50  insertionOpts.loadFromConfigFile(
51  source, sectionNamePrefix + string("_insertOpts"));
52 
53  // [<sectionName>+"_occupancyGrid_##_likelihoodOpts"]
54  likelihoodOpts.loadFromConfigFile(
55  source, sectionNamePrefix + string("_likelihoodOpts"));
56 }
57 
58 void COccupancyGridMap2D::TMapDefinition::dumpToTextStream_map_specific(
59  std::ostream& out) const
60 {
61  LOADABLEOPTS_DUMP_VAR(min_x, float);
62  LOADABLEOPTS_DUMP_VAR(max_x, float);
63  LOADABLEOPTS_DUMP_VAR(min_y, float);
64  LOADABLEOPTS_DUMP_VAR(max_y, float);
65  LOADABLEOPTS_DUMP_VAR(resolution, float);
66 
67  this->insertionOpts.dumpToTextStream(out);
68  this->likelihoodOpts.dumpToTextStream(out);
69 }
70 
71 mrpt::maps::CMetricMap* COccupancyGridMap2D::internal_CreateFromMapDefinition(
73 {
75  *dynamic_cast<const COccupancyGridMap2D::TMapDefinition*>(&_def);
76  auto* obj = new COccupancyGridMap2D(
77  def.min_x, def.max_x, def.min_y, def.max_y, def.resolution);
78  obj->insertionOptions = def.insertionOpts;
79  obj->likelihoodOptions = def.likelihoodOpts;
80  return obj;
81 }
82 // =========== End of Map definition Block =========
83 
85 
86 std::vector<float> COccupancyGridMap2D::entropyTable;
87 
88 static const float MAX_H = 0.69314718055994531f; // ln(2)
89 
90 // Static lookup tables for log-odds
93  COccupancyGridMap2D::get_logodd_lut()
94 {
95  return logodd_lut;
96 }
97 
98 /*---------------------------------------------------------------
99  Constructor
100  ---------------------------------------------------------------*/
101 COccupancyGridMap2D::COccupancyGridMap2D(
102  float min_x, float max_x, float min_y, float max_y, float res)
103  : map(),
104 
105  precomputedLikelihood(),
106 
107  m_basis_map(),
108  m_voronoi_diagram(),
109 
110  updateInfoChangeOnly(),
111  insertionOptions(),
112  likelihoodOptions(),
113  likelihoodOutputs(),
114  CriticalPointsList()
115 {
116  MRPT_START
117  setSize(min_x, max_x, min_y, max_y, res, 0.5f);
118  MRPT_END
119 }
120 
122 {
123  freeMap();
125  x_min = o.x_min;
126  x_max = o.x_max;
127  y_min = o.y_min;
128  y_max = o.y_max;
129  size_x = o.size_x;
130  size_y = o.size_y;
131  map = o.map;
132 
133  m_basis_map.clear();
135 
138 }
139 
141  float xmin, float xmax, float ymin, float ymax, float res,
142  float default_value)
143 {
144  MRPT_START
145 
146  ASSERT_ABOVE_(res, 0.0f);
147  ASSERT_ABOVE_(xmax, xmin);
148  ASSERT_ABOVE_(ymax, ymin);
149  ASSERT_ABOVEEQ_(default_value, 0.0f);
150  ASSERT_BELOWEQ_(default_value, 1.0f);
151 
152  freeMap();
154 
155  // Adjust sizes to adapt them to full sized cells acording to the
156  // resolution:
157  xmin = res * round(xmin / res);
158  ymin = res * round(ymin / res);
159  xmax = res * round(xmax / res);
160  ymax = res * round(ymax / res);
161 
162  // Set parameters:
163  this->resolution = res;
164  this->x_min = xmin;
165  this->x_max = xmax;
166  this->y_min = ymin;
167  this->y_max = ymax;
168 
169  // Now the number of cells should be integers:
170  size_x = round((x_max - x_min) / resolution);
171  size_y = round((y_max - y_min) / resolution);
172 
173 #ifdef ROWSIZE_MULTIPLE_16
174  // map rows must be 16 bytes aligned:
175  if (0 != (size_x % 16))
176  {
177  size_x = ((size_x >> 4) + 1) << 4;
179  }
180  size_x = round((x_max - x_min) / resolution);
181  ASSERT_(0 == (size_x % 16));
182 #endif
183 
184  // Cells memory:
185  map.resize(size_x * size_y, p2l(default_value));
186 
187  // Free these buffers also:
188  m_basis_map.clear();
190 
191  m_is_empty = true;
192 
193  MRPT_END
194 }
195 
197  float new_x_min, float new_x_max, float new_y_min, float new_y_max,
198  float new_cells_default_value, bool additionalMargin) noexcept
199 {
200  unsigned int extra_x_izq = 0, extra_y_arr = 0, new_size_x = 0,
201  new_size_y = 0;
202  std::vector<cellType> new_map;
203 
204  if (new_x_min > new_x_max)
205  {
206  printf(
207  "[COccupancyGridMap2D::resizeGrid] Warning!! Ignoring call, since: "
208  "x_min=%f x_max=%f\n",
209  new_x_min, new_x_max);
210  return;
211  }
212  if (new_y_min > new_y_max)
213  {
214  printf(
215  "[COccupancyGridMap2D::resizeGrid] Warning!! Ignoring call, since: "
216  "y_min=%f y_max=%f\n",
217  new_y_min, new_y_max);
218  return;
219  }
220 
221  // Required?
222  if (new_x_min >= x_min && new_y_min >= y_min && new_x_max <= x_max &&
223  new_y_max <= y_max)
224  return;
225 
226  // For the precomputed likelihood trick:
227  m_likelihoodCacheOutDated = true;
228 
229  // Add an additional margin:
230  if (additionalMargin)
231  {
232  if (new_x_min < x_min) new_x_min = floor(new_x_min - 4);
233  if (new_x_max > x_max) new_x_max = ceil(new_x_max + 4);
234  if (new_y_min < y_min) new_y_min = floor(new_y_min - 4);
235  if (new_y_max > y_max) new_y_max = ceil(new_y_max + 4);
236  }
237 
238  // We do not support grid shrinking... at least stay the same:
239  new_x_min = min(new_x_min, x_min);
240  new_x_max = max(new_x_max, x_max);
241  new_y_min = min(new_y_min, y_min);
242  new_y_max = max(new_y_max, y_max);
243 
244  // Adjust sizes to adapt them to full sized cells acording to the
245  // resolution:
246  if (fabs(new_x_min / resolution - round(new_x_min / resolution)) > 0.05f)
247  new_x_min = resolution * round(new_x_min / resolution);
248  if (fabs(new_y_min / resolution - round(new_y_min / resolution)) > 0.05f)
249  new_y_min = resolution * round(new_y_min / resolution);
250  if (fabs(new_x_max / resolution - round(new_x_max / resolution)) > 0.05f)
251  new_x_max = resolution * round(new_x_max / resolution);
252  if (fabs(new_y_max / resolution - round(new_y_max / resolution)) > 0.05f)
253  new_y_max = resolution * round(new_y_max / resolution);
254 
255  // Change size: 4 sides extensions:
256  extra_x_izq = round((x_min - new_x_min) / resolution);
257  extra_y_arr = round((y_min - new_y_min) / resolution);
258 
259  new_size_x = round((new_x_max - new_x_min) / resolution);
260  new_size_y = round((new_y_max - new_y_min) / resolution);
261 
262  assert(new_size_x >= size_x + extra_x_izq);
263 
264 #ifdef ROWSIZE_MULTIPLE_16
265  // map rows must be 16 bytes aligned:
266  size_t old_new_size_x = new_size_x; // Debug
267  if (0 != (new_size_x % 16))
268  {
269  int size_x_incr = 16 - (new_size_x % 16);
270  // new_x_max = new_x_min + new_size_x * resolution;
271  new_x_max += size_x_incr * resolution;
272  }
273  new_size_x = round((new_x_max - new_x_min) / resolution);
274  assert(0 == (new_size_x % 16));
275 #endif
276 
277  // Reserve new mem block
278  new_map.resize(new_size_x * new_size_y, p2l(new_cells_default_value));
279 
280  // Copy all the old map rows into the new map:
281  {
282  cellType* dest_ptr = &new_map[extra_x_izq + extra_y_arr * new_size_x];
283  cellType* src_ptr = &map[0];
284  size_t row_size = size_x * sizeof(cellType);
285 
286  for (size_t y = 0; y < size_y; y++)
287  {
288 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
289  assert(dest_ptr + row_size - 1 <= &new_map[new_map.size() - 1]);
290  assert(src_ptr + row_size - 1 <= &map[map.size() - 1]);
291 #endif
292  memcpy(dest_ptr, src_ptr, row_size);
293  dest_ptr += new_size_x;
294  src_ptr += size_x;
295  }
296  }
297 
298  // Move new values into the new map:
299  x_min = new_x_min;
300  x_max = new_x_max;
301  y_min = new_y_min;
302  y_max = new_y_max;
303 
304  size_x = new_size_x;
305  size_y = new_size_y;
306 
307  // Free old map, replace by new one:
308  map.swap(new_map);
309 
310  // Free the other buffers:
311  m_basis_map.clear();
312  m_voronoi_diagram.clear();
313 }
314 
315 /*---------------------------------------------------------------
316  freeMap
317  ---------------------------------------------------------------*/
319 {
320  MRPT_START
321 
322  // Free map and sectors
323  map.clear();
324 
325  m_basis_map.clear();
327 
328  size_x = size_y = 0;
329 
330  // For the precomputed likelihood trick:
332 
333  m_is_empty = true;
334 
335  MRPT_END
336 }
337 
338 /*---------------------------------------------------------------
339  Computes the entropy and related values of this grid map.
340  out_H The target variable for absolute entropy, computed
341  as:<br><center>H(map)=Sum<sub>x,y</sub>{ -p(x,y)ln(p(x,y))
342  -(1-p(x,y))ln(1-p(x,y)) }</center><br><br>
343  out_I The target variable for absolute "information", defining I(x) = 1 -
344  H(x)
345  out_mean_H The target variable for mean entropy, defined as entropy per
346  square meter: mean_H(map) = H(map) / (Map length x (meters))(Map length y
347  (meters))
348  out_mean_I The target variable for mean information, defined as information
349  per square meter: mean_I(map) = I(map) / (Map length x (meters))(Map length y
350  (meters))
351  ---------------------------------------------------------------*/
353 {
354 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
355  size_t N = 256;
356 #else
357  size_t N = 65536;
358 #endif
359 
360  // Compute the entropy table: The entropy for each posible cell value
361  // ----------------------------------------------------------------------
362  if (entropyTable.size() != N)
363  {
364  entropyTable.resize(N, 0);
365  for (size_t i = 0; i < N; i++)
366  {
367  const auto p = l2p(static_cast<cellType>(i));
368  auto h = d2f(H(p) + H(1 - p));
369 
370  // Cell's probabilities rounding problem fixing:
371  if (i == 0 || i == (N - 1)) h = 0;
372  if (h > (MAX_H - 1e-4)) h = MAX_H;
373 
374  entropyTable[i] = h;
375  }
376  }
377 
378  // Initialize the global results:
379  info.H = info.I = 0;
380  info.effectiveMappedCells = 0;
381 
382  info.H = info.I = 0;
383  info.effectiveMappedCells = 0;
384  for (signed char it : map)
385  {
386  auto ctu = static_cast<cellTypeUnsigned>(it);
387  auto h = entropyTable[ctu];
388  info.H += h;
389  if (h < (MAX_H - 0.001f))
390  {
391  info.effectiveMappedCells++;
392  info.I -= h;
393  }
394  }
395 
396  // The info: (See ref. paper EMMI in IROS 2006)
397  info.I /= MAX_H;
398  info.I += info.effectiveMappedCells;
399 
400  // Mean values:
401  // ------------------------------------------
402  info.effectiveMappedArea =
404  if (info.effectiveMappedCells)
405  {
406  info.mean_H = info.H / info.effectiveMappedCells;
407  info.mean_I = info.I / info.effectiveMappedCells;
408  }
409  else
410  {
411  info.mean_H = 0;
412  info.mean_I = 0;
413  }
414 }
415 
416 /*---------------------------------------------------------------
417  clear
418  ---------------------------------------------------------------*/
420 {
421  setSize(-10, 10, -10, 10, getResolution());
422  // For the precomputed likelihood trick:
424 }
425 
426 /*---------------------------------------------------------------
427  fill
428  ---------------------------------------------------------------*/
429 void COccupancyGridMap2D::fill(float default_value)
430 {
431  cellType defValue = p2l(default_value);
432  for (auto it = map.begin(); it < map.end(); ++it) *it = defValue;
433  // For the precomputed likelihood trick:
435 }
436 
437 /*---------------------------------------------------------------
438  updateCell
439  ---------------------------------------------------------------*/
440 void COccupancyGridMap2D::updateCell(int x, int y, float v)
441 {
442  // Tip: if x<0, (unsigned)(x) will also be >>> size_x ;-)
443  if (static_cast<unsigned int>(x) >= size_x ||
444  static_cast<unsigned int>(y) >= size_y)
445  return;
446 
447  // Get the current contents of the cell:
448  cellType& theCell = map[x + y * size_x];
449 
450  // Compute the new Bayesian-fused value of the cell:
452  {
453  float old = l2p(theCell);
454  float new_v = 1 / (1 + (1 - v) * (1 - old) / (old * v));
456  updateInfoChangeOnly.I_change += 1 - (H(new_v) + H(1 - new_v)) / MAX_H;
457  }
458  else
459  {
460  cellType obs =
461  p2l(v); // The observation: will be >0 for free, <0 for occupied.
462  if (obs > 0)
463  {
464  if (theCell > (OCCGRID_CELLTYPE_MAX - obs))
465  theCell = OCCGRID_CELLTYPE_MAX; // Saturate
466  else
467  theCell += obs;
468  }
469  else
470  {
471  if (theCell < (OCCGRID_CELLTYPE_MIN - obs))
472  theCell = OCCGRID_CELLTYPE_MIN; // Saturate
473  else
474  theCell += obs;
475  }
476  }
477 }
478 
479 /*---------------------------------------------------------------
480  subSample
481  ---------------------------------------------------------------*/
483 {
484  std::vector<cellType> newMap;
485 
486  ASSERT_(downRatio > 0);
487 
488  resolution *= downRatio;
489 
490  int newSizeX = round((x_max - x_min) / resolution);
491  int newSizeY = round((y_max - y_min) / resolution);
492 
493  newMap.resize(newSizeX * newSizeY);
494 
495  for (int x = 0; x < newSizeX; x++)
496  {
497  for (int y = 0; y < newSizeY; y++)
498  {
499  float newCell = 0;
500 
501  for (int xx = 0; xx < downRatio; xx++)
502  for (int yy = 0; yy < downRatio; yy++)
503  newCell += getCell(x * downRatio + xx, y * downRatio + yy);
504 
505  newCell /= (downRatio * downRatio);
506 
507  newMap[x + y * newSizeX] = p2l(newCell);
508  }
509  }
510 
512  map = newMap;
513 }
514 
515 /*---------------------------------------------------------------
516  computeMatchingWith
517  ---------------------------------------------------------------*/
519  const mrpt::maps::CMetricMap* otherMap2, const CPose2D& otherMapPose_,
520  TMatchingPairList& correspondences, const TMatchingParams& params,
521  TMatchingExtraResults& extraResults) const
522 {
523  MRPT_START
524 
525  extraResults = TMatchingExtraResults();
526 
527  ASSERT_ABOVE_(params.decimation_other_map_points, 0);
529  params.offset_other_map_points, params.decimation_other_map_points);
530 
532  const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);
533 
534  const TPose2D otherMapPose = otherMapPose_.asTPose();
535 
536  const size_t nLocalPoints = otherMap->size();
537  std::vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
538  z_locals(nLocalPoints);
539 
540  const float sin_phi = sin(otherMapPose.phi);
541  const float cos_phi = cos(otherMapPose.phi);
542 
543  size_t nOtherMapPointsWithCorrespondence =
544  0; // Number of points with one corrs. at least
545  size_t nTotalCorrespondences = 0; // Total number of corrs
546  float _sumSqrDist = 0;
547 
548  // The number of cells to look around each point:
549  const int cellsSearchRange =
550  round(params.maxDistForCorrespondence / resolution);
551 
552  // Initially there are no correspondences:
553  correspondences.clear();
554 
555  // Hay mapa local?
556  if (!nLocalPoints) return; // No
557 
558  // Solo hacer matching si existe alguna posibilidad de que
559  // los dos mapas se toquen:
560  // -----------------------------------------------------------
561  float local_x_min = std::numeric_limits<float>::max();
562  float local_x_max = -std::numeric_limits<float>::max();
563  float local_y_min = std::numeric_limits<float>::max();
564  float local_y_max = -std::numeric_limits<float>::max();
565 
566  const auto& otherMap_pxs = otherMap->getPointsBufferRef_x();
567  const auto& otherMap_pys = otherMap->getPointsBufferRef_y();
568  const auto& otherMap_pzs = otherMap->getPointsBufferRef_z();
569 
570  // Translate all local map points:
571  for (unsigned int localIdx = params.offset_other_map_points;
572  localIdx < nLocalPoints;
573  localIdx += params.decimation_other_map_points)
574  {
575  // Girar y desplazar cada uno de los puntos del local map:
576  const float xx = x_locals[localIdx] = otherMapPose.x +
577  cos_phi * otherMap_pxs[localIdx] -
578  sin_phi * otherMap_pys[localIdx];
579  const float yy = y_locals[localIdx] = otherMapPose.y +
580  sin_phi * otherMap_pxs[localIdx] +
581  cos_phi * otherMap_pys[localIdx];
582  z_locals[localIdx] = /* otherMapPose.z +*/ otherMap_pzs[localIdx];
583 
584  // mantener el max/min de los puntos:
585  local_x_min = min(local_x_min, xx);
586  local_x_max = max(local_x_max, xx);
587  local_y_min = min(local_y_min, yy);
588  local_y_max = max(local_y_max, yy);
589  }
590 
591  // If the local map is entirely out of the grid,
592  // do not even try to match them!!
593  if (local_x_min > x_max || local_x_max < x_min || local_y_min > y_max ||
594  local_y_max < y_min)
595  return; // Matching is NULL!
596 
597  const cellType thresholdCellValue = p2l(0.5f);
598 
599  // For each point in the other map:
600  for (unsigned int localIdx = params.offset_other_map_points;
601  localIdx < nLocalPoints;
602  localIdx += params.decimation_other_map_points)
603  {
604  // Starting value:
605  float maxDistForCorrespondenceSquared =
606  square(params.maxDistForCorrespondence);
607 
608  // For speed-up:
609  const float x_local = x_locals[localIdx];
610  const float y_local = y_locals[localIdx];
611  const float z_local = z_locals[localIdx];
612 
613  // Look for the occupied cell closest from the map point:
614  float min_dist = 1e6;
615  TMatchingPair closestCorr;
616 
617  // Get the indexes of cell where the point falls:
618  const int cx0 = x2idx(x_local);
619  const int cy0 = y2idx(y_local);
620 
621  // Get the rectangle to look for into:
622  const int cx_min = max(0, cx0 - cellsSearchRange);
623  const int cx_max =
624  min(static_cast<int>(size_x) - 1, cx0 + cellsSearchRange);
625  const int cy_min = max(0, cy0 - cellsSearchRange);
626  const int cy_max =
627  min(static_cast<int>(size_y) - 1, cy0 + cellsSearchRange);
628 
629  // Will be set to true if a corrs. is found:
630  bool thisLocalHasCorr = false;
631 
632  // Look in nearby cells:
633  for (int cx = cx_min; cx <= cx_max; cx++)
634  {
635  for (int cy = cy_min; cy <= cy_max; cy++)
636  {
637  // Is an occupied cell?
638  if (map[cx + cy * size_x] <
639  thresholdCellValue) // getCell(cx,cy)<0.49)
640  {
641  const float residual_x = idx2x(cx) - x_local;
642  const float residual_y = idx2y(cy) - y_local;
643 
644  // Compute max. allowed distance:
645  maxDistForCorrespondenceSquared = square(
646  params.maxAngularDistForCorrespondence *
647  params.angularDistPivotPoint.distanceTo(
648  TPoint3D(x_local, y_local, 0)) +
649  params.maxDistForCorrespondence);
650 
651  // Square distance to the point:
652  const float this_dist =
653  square(residual_x) + square(residual_y);
654 
655  if (this_dist < maxDistForCorrespondenceSquared)
656  {
657  if (!params.onlyKeepTheClosest)
658  {
659  // save the correspondence:
660  nTotalCorrespondences++;
661  TMatchingPair mp;
662  mp.this_idx = cx + cy * size_x;
663  mp.this_x = idx2x(cx);
664  mp.this_y = idx2y(cy);
665  mp.this_z = z_local;
666  mp.other_idx = localIdx;
667  mp.other_x = otherMap_pxs[localIdx];
668  mp.other_y = otherMap_pys[localIdx];
669  mp.other_z = otherMap_pzs[localIdx];
670  correspondences.push_back(mp);
671  }
672  else
673  {
674  // save the closest only:
675  if (this_dist < min_dist)
676  {
677  min_dist = this_dist;
678 
679  closestCorr.this_idx = cx + cy * size_x;
680  closestCorr.this_x = idx2x(cx);
681  closestCorr.this_y = idx2y(cy);
682  closestCorr.this_z = z_local;
683  closestCorr.other_idx = localIdx;
684  closestCorr.other_x = otherMap_pxs[localIdx];
685  closestCorr.other_y = otherMap_pys[localIdx];
686  closestCorr.other_z = otherMap_pzs[localIdx];
687  }
688  }
689 
690  // At least one:
691  thisLocalHasCorr = true;
692  }
693  }
694  }
695  } // End of find closest nearby cell
696 
697  // save the closest correspondence:
698  if (params.onlyKeepTheClosest &&
699  (min_dist < maxDistForCorrespondenceSquared))
700  {
701  nTotalCorrespondences++;
702  correspondences.push_back(closestCorr);
703  }
704 
705  // At least one corr:
706  if (thisLocalHasCorr)
707  {
708  nOtherMapPointsWithCorrespondence++;
709 
710  // Accumulate the MSE:
711  _sumSqrDist += min_dist;
712  }
713 
714  } // End "for each local point"...
715 
716  extraResults.correspondencesRatio =
717  nOtherMapPointsWithCorrespondence /
718  d2f(nLocalPoints / params.decimation_other_map_points);
719  extraResults.sumSqrDist = _sumSqrDist;
720 
721  MRPT_END
722 }
723 
724 /*---------------------------------------------------------------
725  isEmpty
726  ---------------------------------------------------------------*/
727 bool COccupancyGridMap2D::isEmpty() const { return m_is_empty; }
728 /*---------------------------------------------------------------
729  operator <
730  ---------------------------------------------------------------*/
734 {
735  return e1.first > e2.first;
736 }
737 
738 /*---------------------------------------------------------------
739  computePathCost
740  ---------------------------------------------------------------*/
742  float x1, float y1, float x2, float y2) const
743 {
744  float sumCost = 0;
745 
746  float dist = sqrt(square(x1 - x2) + square(y1 - y2));
747  int nSteps = round(1.5f * dist / resolution);
748 
749  for (int i = 0; i < nSteps; i++)
750  {
751  float x = x1 + (x2 - x1) * i / d2f(nSteps);
752  float y = y1 + (y2 - y1) * i / d2f(nSteps);
753  sumCost += getPos(x, y);
754  }
755 
756  if (nSteps)
757  return sumCost / d2f(nSteps);
758  else
759  return 0;
760 }
761 
763  [[maybe_unused]] const mrpt::maps::CMetricMap* otherMap,
764  [[maybe_unused]] const mrpt::poses::CPose3D& otherMapPose,
765  [[maybe_unused]] const TMatchingRatioParams& params) const
766 {
767  return 0;
768 }
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Parameters for CMetricMap::compute3DMatchingRatio()
static constexpr cellType OCCGRID_CELLTYPE_MIN
Discrete to float conversion factors: The min/max values of the integer cell type, eg.
void clear()
Erase the contents of all the cells.
Definition: CDynamicGrid.h:101
#define MRPT_START
Definition: exceptions.h:241
float getResolution() const
Returns the resolution of the grid map.
mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
double x
X,Y coordinates.
Definition: TPose2D.h:30
double effectiveMappedArea
The target variable for the area of cells with information, i.e.
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
OccGridCellTraits::cellTypeUnsigned cellTypeUnsigned
mrpt::containers::CDynamicGrid< uint16_t > m_voronoi_diagram
Used to store the Voronoi diagram.
void updateCell(int x, int y, float v)
Performs the Bayesian fusion of a new observation of a cell.
static const float MAX_H
mrpt::vision::TStereoCalibParams params
mrpt::containers::CDynamicGrid< uint8_t > m_basis_map
Used for Voronoi calculation.Same struct as "map", but contains a "0" if not a basis point...
struct mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly updateInfoChangeOnly
STL namespace.
static T H(const T p)
Entropy computation internal function:
void freeMap()
Frees the dynamic memory buffers of map.
static constexpr cellType OCCGRID_CELLTYPE_MAX
std::vector< cellType > map
Store of cell occupancy values.
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
float d2f(const double d)
shortcut for static_cast<float>(double)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:65
double H
The target variable for absolute entropy, computed as: H(map)=Sumx,y{ -p(x,y)*ln(p(x,y)) -(1-p(x,y))*ln(1-p(x,y)) }
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.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
bool isEmpty() const override
Returns true upon map construction or after calling clear(), the return changes to false upon success...
std::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
void copyMapContentFrom(const COccupancyGridMap2D &otherMap)
copy the gridmap contents, but not all the options, from another map instance
A list of TMatchingPair.
Definition: TMatchingPair.h:70
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
This namespace contains representation of robot actions and observations.
void subSample(int downRatio)
Performs a downsampling of the gridmap, by a given factor: resolution/=ratio.
double I_change
The cummulative change in Information: This is updated only from the "updateCell" method...
void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
See the base class for more details: In this class it is implemented as correspondences of the passed...
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
static CLogOddsGridMapLUT< COccupancyGridMap2D::cellType > logodd_lut
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double mean_H
The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells) ...
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
return_t square(const num_t x)
Inline function for the square of a number.
#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...
A class for storing an occupancy grid map.
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.
constexpr std::size_t size() const
Definition: TPoseOrPoint.h:67
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
unsigned long effectiveMappedCells
The mapped area in cells.
static cellType 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...
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
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
static std::vector< float > entropyTable
Internally used to speed-up entropy calculation.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
bool m_is_empty
True upon construction; used by isEmpty()
void internal_clear() override
Clear the map: It set all cells to their default occupancy value (0.5), without changing the resoluti...
mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Lightweight 2D pose.
Definition: TPose2D.h:22
float min_x
See COccupancyGridMap2D::COccupancyGridMap2D.
float idx2y(const size_t cy) const
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Definition: CObject.cpp:24
OccGridCellTraits::cellType cellType
The type of the map cells:
void computeEntropy(TEntropyInfo &info) const
Computes the entropy and related values of this grid map.
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.
uint32_t size_x
The size of the grid in cells.
void fill(float default_value=0.5f)
Fills all the cells with a default value.
void resizeGrid(float new_x_min, float new_x_max, float new_y_min, float new_y_max, float new_cells_default_value=0.5f, bool additionalMargin=true) noexcept
Change the size of gridmap, maintaining previous contents.
float x_min
The limits of the grid in "units" (meters)
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:161
void setSize(float x_min, float x_max, float y_min, float y_max, float resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
float computePathCost(float x1, float y1, float x2, float y2) const
Compute the &#39;cost&#39; of traversing a segment of the map according to the occupancy of traversed cells...
double mean_I
The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (ce...
bool operator<(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2)
Parameters for the determination of matchings between point clouds, etc.
Used for returning entropy related information.
Functions for estimating the optimal transformation between two frames of references given measuremen...
int x2idx(float x) const
Transform a coordinate value into a cell index.
double phi
Orientation (rads)
Definition: TPose2D.h:32
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
static float l2p(const cellType l)
Scales an integer representation of the log-odd into a real valued probability in [0...
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
bool enabled
If set to false (default), this struct is not used.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020