MRPT  2.0.3
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 }
ASSERT_ABOVE_
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
mrpt::poses::CPose2D::asTPose
mrpt::math::TPose2D asTPose() const
Definition: CPose2D.cpp:468
COccupancyGridMap2D.h
logodd_lut
static CLogOddsGridMapLUT< COccupancyGridMap2D::cellType > logodd_lut
Definition: COccupancyGridMap2D_common.cpp:91
mrpt::maps::COccupancyGridMap2D::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 the base class for more details: In this class it is implemented as correspondences of the passed...
Definition: COccupancyGridMap2D_common.cpp:518
mrpt::maps::COccupancyGridMap2D::TPairLikelihoodIndex
std::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
Definition: COccupancyGridMap2D.h:628
mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly::enabled
bool enabled
If set to false (default), this struct is not used.
Definition: COccupancyGridMap2D.h:219
mrpt::maps::COccupancyGridMap2D::TMapDefinition::resolution
float resolution
Definition: COccupancyGridMap2D.h:1152
mrpt::math::TPoint3D_< double >
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::COccupancyGridMap2D::cellType
OccGridCellTraits::cellType cellType
The type of the map cells:
Definition: COccupancyGridMap2D.h:60
mrpt::math::TPose2D::phi
double phi
Orientation (rads)
Definition: TPose2D.h:32
mrpt::maps::COccupancyGridMap2D::size_x
uint32_t size_x
The size of the grid in cells.
Definition: COccupancyGridMap2D.h:86
mrpt::tfest::TMatchingPair::other_z
float other_z
Definition: TMatchingPair.h:53
mrpt::maps::COccupancyGridMap2D::y_min
float y_min
Definition: COccupancyGridMap2D.h:88
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::mean_I
double mean_I
The target variable for mean information, defined as information per cell: mean_I(map) = I(map) / (ce...
Definition: COccupancyGridMap2D.h:455
mrpt::maps::COccupancyGridMap2D::size_y
uint32_t size_y
Definition: COccupancyGridMap2D.h:86
mrpt::tfest::TMatchingPair::other_idx
uint32_t other_idx
Definition: TMatchingPair.h:51
mrpt::maps::COccupancyGridMap2D::p2l
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...
Definition: COccupancyGridMap2D.h:341
mrpt::maps::COccupancyGridMap2D::computePathCost
float computePathCost(float x1, float y1, float x2, float y2) const
Compute the 'cost' of traversing a segment of the map according to the occupancy of traversed cells.
Definition: COccupancyGridMap2D_common.cpp:741
MAP_DEFINITION_REGISTER
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
Definition: TMetricMapTypesRegistry.h:91
TPose2D.h
mrpt::maps::COccupancyGridMap2D::resizeGrid
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.
Definition: COccupancyGridMap2D_common.cpp:196
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:64
mrpt::maps::CPointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors.
Definition: CPointsMap.h:67
mrpt::math::TPose2D::y
double y
Definition: TPose2D.h:30
MAX_H
static const float MAX_H
Definition: COccupancyGridMap2D_common.cpp:88
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::maps::COccupancyGridMap2D::internal_clear
void internal_clear() override
Clear the map: It set all cells to their default occupancy value (0.5), without changing the resoluti...
Definition: COccupancyGridMap2D_common.cpp:419
mrpt::tfest
Functions for estimating the optimal transformation between two frames of references given measuremen...
Definition: indiv-compat-decls.h:14
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::maps::COccupancyGridMap2D::TMapDefinition::insertionOpts
mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
Definition: COccupancyGridMap2D.h:1154
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::COccupancyGridMap2D::l2p
static float l2p(const cellType l)
Scales an integer representation of the log-odd into a real valued probability in [0,...
Definition: COccupancyGridMap2D.h:329
mrpt::tfest::TMatchingPair
A structure for holding correspondences between two sets of points or points-like entities in 2D or 3...
Definition: TMatchingPair.h:31
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::maps::operator<
bool operator<(const COccupancyGridMap2D::TPairLikelihoodIndex &e1, const COccupancyGridMap2D::TPairLikelihoodIndex &e2)
Definition: COccupancyGridMap2D_common.cpp:731
mrpt::maps::COccupancyGridMap2D::entropyTable
static std::vector< float > entropyTable
Internally used to speed-up entropy calculation.
Definition: COccupancyGridMap2D.h:127
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::maps::COccupancyGridMap2D::y_max
float y_max
Definition: COccupancyGridMap2D.h:88
mrpt::maps::COccupancyGridMap2D::m_is_empty
bool m_is_empty
True upon construction; used by isEmpty()
Definition: COccupancyGridMap2D.h:109
mrpt::maps::COccupancyGridMap2D::idx2y
float idx2y(const size_t cy) const
Definition: COccupancyGridMap2D.h:311
mrpt::maps::COccupancyGridMap2D::getCell
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
Definition: COccupancyGridMap2D.h:357
mrpt::maps::COccupancyGridMap2D::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: COccupancyGridMap2D_common.cpp:762
mrpt::maps::CLogOddsGridMapLUT
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
Definition: CLogOddsGridMapLUT.h:27
mrpt::maps::COccupancyGridMap2D::computeEntropy
void computeEntropy(TEntropyInfo &info) const
Computes the entropy and related values of this grid map.
Definition: COccupancyGridMap2D_common.cpp:352
ASSERT_BELOW_
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
mrpt::maps::COccupancyGridMap2D::TMapDefinition
Definition: COccupancyGridMap2D.h:1149
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
mrpt::maps::COccupancyGridMap2D::fill
void fill(float default_value=0.5f)
Fills all the cells with a default value.
Definition: COccupancyGridMap2D_common.cpp:429
mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly::I_change
double I_change
The cummulative change in Information: This is updated only from the "updateCell" method.
Definition: COccupancyGridMap2D.h:222
mrpt::maps::COccupancyGridMap2D::getPos
float getPos(float x, float y) const
Read the real valued [0,1] contents of a cell, given its coordinates.
Definition: COccupancyGridMap2D.h:394
mrpt::maps::COccupancyGridMap2D::x_min
float x_min
The limits of the grid in "units" (meters)
Definition: COccupancyGridMap2D.h:88
mrpt::maps::COccupancyGridMap2D::setSize
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.
Definition: COccupancyGridMap2D_common.cpp:140
ASSERT_BELOWEQ_
#define ASSERT_BELOWEQ_(__A, __B)
Definition: exceptions.h:161
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::H
double H
The target variable for absolute entropy, computed as:
Definition: COccupancyGridMap2D.h:446
mrpt::maps::COccupancyGridMap2D::OCCGRID_CELLTYPE_MIN
static constexpr cellType OCCGRID_CELLTYPE_MIN
Discrete to float conversion factors: The min/max values of the integer cell type,...
Definition: COccupancyGridMap2D.h:65
mrpt::tfest::TMatchingPair::this_x
float this_x
Definition: TMatchingPair.h:52
mrpt::tfest::TMatchingPair::this_idx
uint32_t this_idx
Definition: TMatchingPair.h:50
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
mrpt::maps::COccupancyGridMap2D::x_max
float x_max
Definition: COccupancyGridMap2D.h:88
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::maps::COccupancyGridMap2D::cellTypeUnsigned
OccGridCellTraits::cellTypeUnsigned cellTypeUnsigned
Definition: COccupancyGridMap2D.h:61
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::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::maps::COccupancyGridMap2D::y2idx
int y2idx(float y) const
Definition: COccupancyGridMap2D.h:292
mrpt::tfest::TMatchingPair::other_y
float other_y
Definition: TMatchingPair.h:53
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::maps::COccupancyGridMap2D::x2idx
int x2idx(float x) const
Transform a coordinate value into a cell index.
Definition: COccupancyGridMap2D.h:288
mrpt::maps::COccupancyGridMap2D::TMapDefinition::max_y
float max_y
Definition: COccupancyGridMap2D.h:1151
mrpt::rtti::TRuntimeClassId::derivedFrom
bool derivedFrom(const TRuntimeClassId *pBaseClass) const
Definition: CObject.cpp:24
mrpt::maps::COccupancyGridMap2D::TMapDefinition::min_y
float min_y
Definition: COccupancyGridMap2D.h:1151
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::effectiveMappedCells
unsigned long effectiveMappedCells
The mapped area in cells.
Definition: COccupancyGridMap2D.h:460
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::I
double I
The target variable for absolute "information", defining I(x) = 1 - H(x)
Definition: COccupancyGridMap2D.h:449
mrpt::math::TPose2D
Lightweight 2D pose.
Definition: TPose2D.h:22
mrpt::d2f
float d2f(const double d)
shortcut for static_cast<float>(double)
Definition: core/include/mrpt/core/bits_math.h:189
mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly::cellsUpdated
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
Definition: COccupancyGridMap2D.h:225
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
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::COccupancyGridMap2D::TEntropyInfo::mean_H
double mean_H
The target variable for mean entropy, defined as entropy per cell: mean_H(map) = H(map) / (cells)
Definition: COccupancyGridMap2D.h:452
mrpt::math::internal::ProvideStaticResize::size
constexpr std::size_t size() const
Definition: TPoseOrPoint.h:67
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::maps::COccupancyGridMap2D::OCCGRID_CELLTYPE_MAX
static constexpr cellType OCCGRID_CELLTYPE_MAX
Definition: COccupancyGridMap2D.h:67
IMPLEMENTS_SERIALIZABLE
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
Definition: CSerializable.h:166
mrpt::maps::COccupancyGridMap2D::map
std::vector< cellType > map
Store of cell occupancy values.
Definition: COccupancyGridMap2D.h:84
mrpt::maps::TMatchingParams
Parameters for the determination of matchings between point clouds, etc.
Definition: metric_map_types.h:20
mrpt::tfest::TMatchingPair::this_y
float this_y
Definition: TMatchingPair.h:52
mrpt::maps::COccupancyGridMap2D::TEntropyInfo
Used for returning entropy related information.
Definition: COccupancyGridMap2D.h:441
CPose3D.h
mrpt::maps::COccupancyGridMap2D::H
static T H(const T p)
Entropy computation internal function:
Definition: COccupancyGridMap2D.h:119
ASSERT_ABOVEEQ_
#define ASSERT_ABOVEEQ_(__A, __B)
Definition: exceptions.h:167
mrpt::maps::COccupancyGridMap2D::resolution
float resolution
Cell size, i.e.
Definition: COccupancyGridMap2D.h:90
mrpt::maps::COccupancyGridMap2D::TEntropyInfo::effectiveMappedArea
double effectiveMappedArea
The target variable for the area of cells with information, i.e.
Definition: COccupancyGridMap2D.h:458
CLASS_ID
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Definition: CObject.h:102
mrpt::maps::COccupancyGridMap2D::getResolution
float getResolution() const
Returns the resolution of the grid map.
Definition: COccupancyGridMap2D.h:286
mrpt::maps::COccupancyGridMap2D::TMapDefinition::max_x
float max_x
Definition: COccupancyGridMap2D.h:1151
mrpt::maps::COccupancyGridMap2D::freeMap
void freeMap()
Frees the dynamic memory buffers of map.
Definition: COccupancyGridMap2D_common.cpp:318
mrpt::maps::COccupancyGridMap2D::m_basis_map
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.
Definition: COccupancyGridMap2D.h:100
maps-precomp.h
mrpt::maps::COccupancyGridMap2D::isEmpty
bool isEmpty() const override
Returns true upon map construction or after calling clear(), the return changes to false upon success...
Definition: COccupancyGridMap2D_common.cpp:727
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::CMetricMap::GetRuntimeClass
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
mrpt::tfest::TMatchingPair::this_z
float this_z
Definition: TMatchingPair.h:52
mrpt::maps::COccupancyGridMap2D::idx2x
float idx2x(const size_t cx) const
Transform a cell index into a coordinate value.
Definition: COccupancyGridMap2D.h:307
mrpt::maps::COccupancyGridMap2D::m_likelihoodCacheOutDated
bool m_likelihoodCacheOutDated
Definition: COccupancyGridMap2D.h:96
mrpt::maps::COccupancyGridMap2D::m_voronoi_diagram
mrpt::containers::CDynamicGrid< uint16_t > m_voronoi_diagram
Used to store the Voronoi diagram.
Definition: COccupancyGridMap2D.h:106
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::containers::CDynamicGrid::clear
void clear()
Erase the contents of all the cells.
Definition: CDynamicGrid.h:101
mrpt::tfest::TMatchingPairList
A list of TMatchingPair.
Definition: TMatchingPair.h:70
mrpt::maps::COccupancyGridMap2D::subSample
void subSample(int downRatio)
Performs a downsampling of the gridmap, by a given factor: resolution/=ratio.
Definition: COccupancyGridMap2D_common.cpp:482
mrpt::maps::COccupancyGridMap2D::TMapDefinition::likelihoodOpts
mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Definition: COccupancyGridMap2D.h:1156
mrpt::maps::COccupancyGridMap2D::updateCell
void updateCell(int x, int y, float v)
Performs the Bayesian fusion of a new observation of a cell.
Definition: COccupancyGridMap2D_common.cpp:440
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
CSimplePointsMap.h
mrpt::maps::COccupancyGridMap2D::updateInfoChangeOnly
struct mrpt::maps::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly updateInfoChangeOnly
mrpt::maps::COccupancyGridMap2D::copyMapContentFrom
void copyMapContentFrom(const COccupancyGridMap2D &otherMap)
copy the gridmap contents, but not all the options, from another map instance
Definition: COccupancyGridMap2D_common.cpp:121
mrpt::maps::COccupancyGridMap2D::TMapDefinition::min_x
float min_x
See COccupancyGridMap2D::COccupancyGridMap2D.
Definition: COccupancyGridMap2D.h:1151
mrpt::math::TPose2D::x
double x
X,Y coordinates.
Definition: TPose2D.h:30
mrpt::tfest::TMatchingPair::other_x
float other_x
Definition: TMatchingPair.h:53
mrpt::system::os::memcpy
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Thu May 21 21:53:32 UTC 2020