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-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "maps-precomp.h" // Precomp header
11 
12 // Force size_x being a multiple of 16 cells
13 //#define ROWSIZE_MULTIPLE_16
14 
17 #include <mrpt/poses/CPose3D.h>
19 
20 using namespace mrpt;
21 using namespace mrpt::math;
22 using namespace mrpt::maps;
23 using namespace mrpt::obs;
24 using namespace mrpt::poses;
25 using namespace mrpt::tfest;
26 using namespace std;
27 
28 // =========== Begin of Map definition ============
30  "COccupancyGridMap2D,occupancyGrid", mrpt::maps::COccupancyGridMap2D)
31 
32 COccupancyGridMap2D::TMapDefinition::TMapDefinition() = default;
33 
34 void COccupancyGridMap2D::TMapDefinition::loadFromConfigFile_map_specific(
36  const std::string& sectionNamePrefix)
37 {
38  // [<sectionNamePrefix>+"_creationOpts"]
39  const std::string sSectCreation =
40  sectionNamePrefix + string("_creationOpts");
41  MRPT_LOAD_CONFIG_VAR(min_x, float, source, sSectCreation);
42  MRPT_LOAD_CONFIG_VAR(max_x, float, source, sSectCreation);
43  MRPT_LOAD_CONFIG_VAR(min_y, float, source, sSectCreation);
44  MRPT_LOAD_CONFIG_VAR(max_y, float, source, sSectCreation);
45  MRPT_LOAD_CONFIG_VAR(resolution, float, source, sSectCreation);
46 
47  // [<sectionName>+"_occupancyGrid_##_insertOpts"]
48  insertionOpts.loadFromConfigFile(
49  source, sectionNamePrefix + string("_insertOpts"));
50 
51  // [<sectionName>+"_occupancyGrid_##_likelihoodOpts"]
52  likelihoodOpts.loadFromConfigFile(
53  source, sectionNamePrefix + string("_likelihoodOpts"));
54 }
55 
56 void COccupancyGridMap2D::TMapDefinition::dumpToTextStream_map_specific(
57  std::ostream& out) const
58 {
59  LOADABLEOPTS_DUMP_VAR(min_x, float);
60  LOADABLEOPTS_DUMP_VAR(max_x, float);
61  LOADABLEOPTS_DUMP_VAR(min_y, float);
62  LOADABLEOPTS_DUMP_VAR(max_y, float);
63  LOADABLEOPTS_DUMP_VAR(resolution, float);
64 
65  this->insertionOpts.dumpToTextStream(out);
66  this->likelihoodOpts.dumpToTextStream(out);
67 }
68 
69 mrpt::maps::CMetricMap* COccupancyGridMap2D::internal_CreateFromMapDefinition(
71 {
73  *dynamic_cast<const COccupancyGridMap2D::TMapDefinition*>(&_def);
74  auto* obj = new COccupancyGridMap2D(
75  def.min_x, def.max_x, def.min_y, def.max_y, def.resolution);
76  obj->insertionOptions = def.insertionOpts;
77  obj->likelihoodOptions = def.likelihoodOpts;
78  return obj;
79 }
80 // =========== End of Map definition Block =========
81 
83 
84 std::vector<float> COccupancyGridMap2D::entropyTable;
85 
86 static const float MAX_H = 0.69314718055994531f; // ln(2)
87 
88 // Static lookup tables for log-odds
91  COccupancyGridMap2D::get_logodd_lut()
92 {
93  return logodd_lut;
94 }
95 
96 /*---------------------------------------------------------------
97  Constructor
98  ---------------------------------------------------------------*/
99 COccupancyGridMap2D::COccupancyGridMap2D(
100  float min_x, float max_x, float min_y, float max_y, float res)
101  : map(),
102 
103  precomputedLikelihood(),
104 
105  m_basis_map(),
106  m_voronoi_diagram(),
107 
108  updateInfoChangeOnly(),
109  insertionOptions(),
110  likelihoodOptions(),
111  likelihoodOutputs(),
112  CriticalPointsList()
113 {
114  MRPT_START
115  setSize(min_x, max_x, min_y, max_y, res, 0.5f);
116  MRPT_END
117 }
118 
120 {
121  freeMap();
123  x_min = o.x_min;
124  x_max = o.x_max;
125  y_min = o.y_min;
126  y_max = o.y_max;
127  size_x = o.size_x;
128  size_y = o.size_y;
129  map = o.map;
130 
131  m_basis_map.clear();
133 
136 }
137 
139  float xmin, float xmax, float ymin, float ymax, float res,
140  float default_value)
141 {
142  MRPT_START
143 
144  ASSERT_ABOVE_(res, 0.0f);
145  ASSERT_ABOVE_(xmax, xmin);
146  ASSERT_ABOVE_(ymax, ymin);
147  ASSERT_ABOVEEQ_(default_value, 0.0f);
148  ASSERT_BELOWEQ_(default_value, 1.0f);
149 
150  freeMap();
152 
153  // Adjust sizes to adapt them to full sized cells acording to the
154  // resolution:
155  xmin = res * round(xmin / res);
156  ymin = res * round(ymin / res);
157  xmax = res * round(xmax / res);
158  ymax = res * round(ymax / res);
159 
160  // Set parameters:
161  this->resolution = res;
162  this->x_min = xmin;
163  this->x_max = xmax;
164  this->y_min = ymin;
165  this->y_max = ymax;
166 
167  // Now the number of cells should be integers:
168  size_x = round((x_max - x_min) / resolution);
169  size_y = round((y_max - y_min) / resolution);
170 
171 #ifdef ROWSIZE_MULTIPLE_16
172  // map rows must be 16 bytes aligned:
173  if (0 != (size_x % 16))
174  {
175  size_x = ((size_x >> 4) + 1) << 4;
177  }
178  size_x = round((x_max - x_min) / resolution);
179  ASSERT_(0 == (size_x % 16));
180 #endif
181 
182  // Cells memory:
183  map.resize(size_x * size_y, p2l(default_value));
184 
185  // Free these buffers also:
186  m_basis_map.clear();
188 
189  m_is_empty = true;
190 
191  MRPT_END
192 }
193 
195  float new_x_min, float new_x_max, float new_y_min, float new_y_max,
196  float new_cells_default_value, bool additionalMargin) noexcept
197 {
198  unsigned int extra_x_izq = 0, extra_y_arr = 0, new_size_x = 0,
199  new_size_y = 0;
200  std::vector<cellType> new_map;
201 
202  if (new_x_min > new_x_max)
203  {
204  printf(
205  "[COccupancyGridMap2D::resizeGrid] Warning!! Ignoring call, since: "
206  "x_min=%f x_max=%f\n",
207  new_x_min, new_x_max);
208  return;
209  }
210  if (new_y_min > new_y_max)
211  {
212  printf(
213  "[COccupancyGridMap2D::resizeGrid] Warning!! Ignoring call, since: "
214  "y_min=%f y_max=%f\n",
215  new_y_min, new_y_max);
216  return;
217  }
218 
219  // Required?
220  if (new_x_min >= x_min && new_y_min >= y_min && new_x_max <= x_max &&
221  new_y_max <= y_max)
222  return;
223 
224  // For the precomputed likelihood trick:
225  m_likelihoodCacheOutDated = true;
226 
227  // Add an additional margin:
228  if (additionalMargin)
229  {
230  if (new_x_min < x_min) new_x_min = floor(new_x_min - 4);
231  if (new_x_max > x_max) new_x_max = ceil(new_x_max + 4);
232  if (new_y_min < y_min) new_y_min = floor(new_y_min - 4);
233  if (new_y_max > y_max) new_y_max = ceil(new_y_max + 4);
234  }
235 
236  // We do not support grid shrinking... at least stay the same:
237  new_x_min = min(new_x_min, x_min);
238  new_x_max = max(new_x_max, x_max);
239  new_y_min = min(new_y_min, y_min);
240  new_y_max = max(new_y_max, y_max);
241 
242  // Adjust sizes to adapt them to full sized cells acording to the
243  // resolution:
244  if (fabs(new_x_min / resolution - round(new_x_min / resolution)) > 0.05f)
245  new_x_min = resolution * round(new_x_min / resolution);
246  if (fabs(new_y_min / resolution - round(new_y_min / resolution)) > 0.05f)
247  new_y_min = resolution * round(new_y_min / resolution);
248  if (fabs(new_x_max / resolution - round(new_x_max / resolution)) > 0.05f)
249  new_x_max = resolution * round(new_x_max / resolution);
250  if (fabs(new_y_max / resolution - round(new_y_max / resolution)) > 0.05f)
251  new_y_max = resolution * round(new_y_max / resolution);
252 
253  // Change size: 4 sides extensions:
254  extra_x_izq = round((x_min - new_x_min) / resolution);
255  extra_y_arr = round((y_min - new_y_min) / resolution);
256 
257  new_size_x = round((new_x_max - new_x_min) / resolution);
258  new_size_y = round((new_y_max - new_y_min) / resolution);
259 
260  assert(new_size_x >= size_x + extra_x_izq);
261 
262 #ifdef ROWSIZE_MULTIPLE_16
263  // map rows must be 16 bytes aligned:
264  size_t old_new_size_x = new_size_x; // Debug
265  if (0 != (new_size_x % 16))
266  {
267  int size_x_incr = 16 - (new_size_x % 16);
268  // new_x_max = new_x_min + new_size_x * resolution;
269  new_x_max += size_x_incr * resolution;
270  }
271  new_size_x = round((new_x_max - new_x_min) / resolution);
272  assert(0 == (new_size_x % 16));
273 #endif
274 
275  // Reserve new mem block
276  new_map.resize(new_size_x * new_size_y, p2l(new_cells_default_value));
277 
278  // Copy all the old map rows into the new map:
279  {
280  cellType* dest_ptr = &new_map[extra_x_izq + extra_y_arr * new_size_x];
281  cellType* src_ptr = &map[0];
282  size_t row_size = size_x * sizeof(cellType);
283 
284  for (size_t y = 0; y < size_y; y++)
285  {
286 #if defined(_DEBUG) || (MRPT_ALWAYS_CHECKS_DEBUG)
287  assert(dest_ptr + row_size - 1 <= &new_map[new_map.size() - 1]);
288  assert(src_ptr + row_size - 1 <= &map[map.size() - 1]);
289 #endif
290  memcpy(dest_ptr, src_ptr, row_size);
291  dest_ptr += new_size_x;
292  src_ptr += size_x;
293  }
294  }
295 
296  // Move new values into the new map:
297  x_min = new_x_min;
298  x_max = new_x_max;
299  y_min = new_y_min;
300  y_max = new_y_max;
301 
302  size_x = new_size_x;
303  size_y = new_size_y;
304 
305  // Free old map, replace by new one:
306  map.swap(new_map);
307 
308  // Free the other buffers:
309  m_basis_map.clear();
310  m_voronoi_diagram.clear();
311 }
312 
313 /*---------------------------------------------------------------
314  freeMap
315  ---------------------------------------------------------------*/
317 {
318  MRPT_START
319 
320  // Free map and sectors
321  map.clear();
322 
323  m_basis_map.clear();
325 
326  size_x = size_y = 0;
327 
328  // For the precomputed likelihood trick:
330 
331  m_is_empty = true;
332 
333  MRPT_END
334 }
335 
336 /*---------------------------------------------------------------
337  Computes the entropy and related values of this grid map.
338  out_H The target variable for absolute entropy, computed
339  as:<br><center>H(map)=Sum<sub>x,y</sub>{ -p(x,y)ln(p(x,y))
340  -(1-p(x,y))ln(1-p(x,y)) }</center><br><br>
341  out_I The target variable for absolute "information", defining I(x) = 1 -
342  H(x)
343  out_mean_H The target variable for mean entropy, defined as entropy per
344  square meter: mean_H(map) = H(map) / (Map length x (meters))(Map length y
345  (meters))
346  out_mean_I The target variable for mean information, defined as information
347  per square meter: mean_I(map) = I(map) / (Map length x (meters))(Map length y
348  (meters))
349  ---------------------------------------------------------------*/
351 {
352  unsigned long i;
353  float h, p;
354 
355 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
356  unsigned int N = 256;
357 #else
358  unsigned int N = 65536;
359 #endif
360 
361  // Compute the entropy table: The entropy for each posible cell value
362  // ----------------------------------------------------------------------
363  if (entropyTable.size() != N)
364  {
365  entropyTable.resize(N, 0);
366  for (i = 0; i < N; i++)
367  {
368  p = l2p(static_cast<cellType>(i));
369  h = H(p) + H(1 - p);
370 
371  // Cell's probabilities rounding problem fixing:
372  if (i == 0 || i == (N - 1)) h = 0;
373  if (h > (MAX_H - 1e-4)) h = MAX_H;
374 
375  entropyTable[i] = h;
376  }
377  }
378 
379  // Initialize the global results:
380  info.H = info.I = 0;
381  info.effectiveMappedCells = 0;
382 
383  info.H = info.I = 0;
384  info.effectiveMappedCells = 0;
385  for (signed char it : map)
386  {
387  auto ctu = static_cast<cellTypeUnsigned>(it);
388  h = entropyTable[ctu];
389  info.H += h;
390  if (h < (MAX_H - 0.001f))
391  {
392  info.effectiveMappedCells++;
393  info.I -= h;
394  }
395  }
396 
397  // The info: (See ref. paper EMMI in IROS 2006)
398  info.I /= MAX_H;
399  info.I += info.effectiveMappedCells;
400 
401  // Mean values:
402  // ------------------------------------------
403  info.effectiveMappedArea =
405  if (info.effectiveMappedCells)
406  {
407  info.mean_H = info.H / info.effectiveMappedCells;
408  info.mean_I = info.I / info.effectiveMappedCells;
409  }
410  else
411  {
412  info.mean_H = 0;
413  info.mean_I = 0;
414  }
415 }
416 
417 /*---------------------------------------------------------------
418  Entropy aux. function
419  ---------------------------------------------------------------*/
420 double COccupancyGridMap2D::H(double p)
421 {
422  if (p == 0 || p == 1)
423  return 0;
424  else
425  return -p * log(p);
426 }
427 
428 /*---------------------------------------------------------------
429  clear
430  ---------------------------------------------------------------*/
432 {
433  setSize(-10, 10, -10, 10, getResolution());
434  // resetFeaturesCache();
435  // For the precomputed likelihood trick:
437 }
438 
439 /*---------------------------------------------------------------
440  fill
441  ---------------------------------------------------------------*/
442 void COccupancyGridMap2D::fill(float default_value)
443 {
444  cellType defValue = p2l(default_value);
445  for (auto it = map.begin(); it < map.end(); ++it) *it = defValue;
446  // For the precomputed likelihood trick:
448  // resetFeaturesCache();
449 }
450 
451 /*---------------------------------------------------------------
452  updateCell
453  ---------------------------------------------------------------*/
454 void COccupancyGridMap2D::updateCell(int x, int y, float v)
455 {
456  // Tip: if x<0, (unsigned)(x) will also be >>> size_x ;-)
457  if (static_cast<unsigned int>(x) >= size_x ||
458  static_cast<unsigned int>(y) >= size_y)
459  return;
460 
461  // Get the current contents of the cell:
462  cellType& theCell = map[x + y * size_x];
463 
464  // Compute the new Bayesian-fused value of the cell:
466  {
467  float old = l2p(theCell);
468  float new_v = 1 / (1 + (1 - v) * (1 - old) / (old * v));
470  updateInfoChangeOnly.I_change += 1 - (H(new_v) + H(1 - new_v)) / MAX_H;
471  }
472  else
473  {
474  cellType obs =
475  p2l(v); // The observation: will be >0 for free, <0 for occupied.
476  if (obs > 0)
477  {
478  if (theCell > (OCCGRID_CELLTYPE_MAX - obs))
479  theCell = OCCGRID_CELLTYPE_MAX; // Saturate
480  else
481  theCell += obs;
482  }
483  else
484  {
485  if (theCell < (OCCGRID_CELLTYPE_MIN - obs))
486  theCell = OCCGRID_CELLTYPE_MIN; // Saturate
487  else
488  theCell += obs;
489  }
490  }
491 }
492 
493 /*---------------------------------------------------------------
494  subSample
495  ---------------------------------------------------------------*/
497 {
498  std::vector<cellType> newMap;
499 
500  ASSERT_(downRatio > 0);
501 
502  resolution *= downRatio;
503 
504  int newSizeX = round((x_max - x_min) / resolution);
505  int newSizeY = round((y_max - y_min) / resolution);
506 
507  newMap.resize(newSizeX * newSizeY);
508 
509  for (int x = 0; x < newSizeX; x++)
510  {
511  for (int y = 0; y < newSizeY; y++)
512  {
513  float newCell = 0;
514 
515  for (int xx = 0; xx < downRatio; xx++)
516  for (int yy = 0; yy < downRatio; yy++)
517  newCell += getCell(x * downRatio + xx, y * downRatio + yy);
518 
519  newCell /= (downRatio * downRatio);
520 
521  newMap[x + y * newSizeX] = p2l(newCell);
522  }
523  }
524 
526  map = newMap;
527 }
528 
529 /*---------------------------------------------------------------
530  computeMatchingWith
531  ---------------------------------------------------------------*/
533  const mrpt::maps::CMetricMap* otherMap2, const CPose2D& otherMapPose_,
534  TMatchingPairList& correspondences, const TMatchingParams& params,
535  TMatchingExtraResults& extraResults) const
536 {
537  MRPT_START
538 
539  extraResults = TMatchingExtraResults();
540 
541  ASSERT_ABOVE_(params.decimation_other_map_points, 0);
543  params.offset_other_map_points, params.decimation_other_map_points);
544 
546  const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);
547 
548  const TPose2D otherMapPose = otherMapPose_.asTPose();
549 
550  const size_t nLocalPoints = otherMap->size();
551  std::vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
552  z_locals(nLocalPoints);
553 
554  const float sin_phi = sin(otherMapPose.phi);
555  const float cos_phi = cos(otherMapPose.phi);
556 
557  size_t nOtherMapPointsWithCorrespondence =
558  0; // Number of points with one corrs. at least
559  size_t nTotalCorrespondences = 0; // Total number of corrs
560  float _sumSqrDist = 0;
561 
562  // The number of cells to look around each point:
563  const int cellsSearchRange =
564  round(params.maxDistForCorrespondence / resolution);
565 
566  // Initially there are no correspondences:
567  correspondences.clear();
568 
569  // Hay mapa local?
570  if (!nLocalPoints) return; // No
571 
572  // Solo hacer matching si existe alguna posibilidad de que
573  // los dos mapas se toquen:
574  // -----------------------------------------------------------
575  float local_x_min = std::numeric_limits<float>::max();
576  float local_x_max = -std::numeric_limits<float>::max();
577  float local_y_min = std::numeric_limits<float>::max();
578  float local_y_max = -std::numeric_limits<float>::max();
579 
580  const auto& otherMap_pxs = otherMap->getPointsBufferRef_x();
581  const auto& otherMap_pys = otherMap->getPointsBufferRef_y();
582  const auto& otherMap_pzs = otherMap->getPointsBufferRef_z();
583 
584  // Translate all local map points:
585  for (unsigned int localIdx = params.offset_other_map_points;
586  localIdx < nLocalPoints;
587  localIdx += params.decimation_other_map_points)
588  {
589  // Girar y desplazar cada uno de los puntos del local map:
590  const float xx = x_locals[localIdx] = otherMapPose.x +
591  cos_phi * otherMap_pxs[localIdx] -
592  sin_phi * otherMap_pys[localIdx];
593  const float yy = y_locals[localIdx] = otherMapPose.y +
594  sin_phi * otherMap_pxs[localIdx] +
595  cos_phi * otherMap_pys[localIdx];
596  z_locals[localIdx] = /* otherMapPose.z +*/ otherMap_pzs[localIdx];
597 
598  // mantener el max/min de los puntos:
599  local_x_min = min(local_x_min, xx);
600  local_x_max = max(local_x_max, xx);
601  local_y_min = min(local_y_min, yy);
602  local_y_max = max(local_y_max, yy);
603  }
604 
605  // If the local map is entirely out of the grid,
606  // do not even try to match them!!
607  if (local_x_min > x_max || local_x_max < x_min || local_y_min > y_max ||
608  local_y_max < y_min)
609  return; // Matching is NULL!
610 
611  const cellType thresholdCellValue = p2l(0.5f);
612 
613  // For each point in the other map:
614  for (unsigned int localIdx = params.offset_other_map_points;
615  localIdx < nLocalPoints;
616  localIdx += params.decimation_other_map_points)
617  {
618  // Starting value:
619  float maxDistForCorrespondenceSquared =
620  square(params.maxDistForCorrespondence);
621 
622  // For speed-up:
623  const float x_local = x_locals[localIdx];
624  const float y_local = y_locals[localIdx];
625  const float z_local = z_locals[localIdx];
626 
627  // Look for the occupied cell closest from the map point:
628  float min_dist = 1e6;
629  TMatchingPair closestCorr;
630 
631  // Get the indexes of cell where the point falls:
632  const int cx0 = x2idx(x_local);
633  const int cy0 = y2idx(y_local);
634 
635  // Get the rectangle to look for into:
636  const int cx_min = max(0, cx0 - cellsSearchRange);
637  const int cx_max =
638  min(static_cast<int>(size_x) - 1, cx0 + cellsSearchRange);
639  const int cy_min = max(0, cy0 - cellsSearchRange);
640  const int cy_max =
641  min(static_cast<int>(size_y) - 1, cy0 + cellsSearchRange);
642 
643  // Will be set to true if a corrs. is found:
644  bool thisLocalHasCorr = false;
645 
646  // Look in nearby cells:
647  for (int cx = cx_min; cx <= cx_max; cx++)
648  {
649  for (int cy = cy_min; cy <= cy_max; cy++)
650  {
651  // Is an occupied cell?
652  if (map[cx + cy * size_x] <
653  thresholdCellValue) // getCell(cx,cy)<0.49)
654  {
655  const float residual_x = idx2x(cx) - x_local;
656  const float residual_y = idx2y(cy) - y_local;
657 
658  // Compute max. allowed distance:
659  maxDistForCorrespondenceSquared = square(
660  params.maxAngularDistForCorrespondence *
661  params.angularDistPivotPoint.distanceTo(
662  TPoint3D(x_local, y_local, 0)) +
663  params.maxDistForCorrespondence);
664 
665  // Square distance to the point:
666  const float this_dist =
667  square(residual_x) + square(residual_y);
668 
669  if (this_dist < maxDistForCorrespondenceSquared)
670  {
671  if (!params.onlyKeepTheClosest)
672  {
673  // save the correspondence:
674  nTotalCorrespondences++;
675  TMatchingPair mp;
676  mp.this_idx = cx + cy * size_x;
677  mp.this_x = idx2x(cx);
678  mp.this_y = idx2y(cy);
679  mp.this_z = z_local;
680  mp.other_idx = localIdx;
681  mp.other_x = otherMap_pxs[localIdx];
682  mp.other_y = otherMap_pys[localIdx];
683  mp.other_z = otherMap_pzs[localIdx];
684  correspondences.push_back(mp);
685  }
686  else
687  {
688  // save the closest only:
689  if (this_dist < min_dist)
690  {
691  min_dist = this_dist;
692 
693  closestCorr.this_idx = cx + cy * size_x;
694  closestCorr.this_x = idx2x(cx);
695  closestCorr.this_y = idx2y(cy);
696  closestCorr.this_z = z_local;
697  closestCorr.other_idx = localIdx;
698  closestCorr.other_x = otherMap_pxs[localIdx];
699  closestCorr.other_y = otherMap_pys[localIdx];
700  closestCorr.other_z = otherMap_pzs[localIdx];
701  }
702  }
703 
704  // At least one:
705  thisLocalHasCorr = true;
706  }
707  }
708  }
709  } // End of find closest nearby cell
710 
711  // save the closest correspondence:
712  if (params.onlyKeepTheClosest &&
713  (min_dist < maxDistForCorrespondenceSquared))
714  {
715  nTotalCorrespondences++;
716  correspondences.push_back(closestCorr);
717  }
718 
719  // At least one corr:
720  if (thisLocalHasCorr)
721  {
722  nOtherMapPointsWithCorrespondence++;
723 
724  // Accumulate the MSE:
725  _sumSqrDist += min_dist;
726  }
727 
728  } // End "for each local point"...
729 
730  extraResults.correspondencesRatio =
731  nOtherMapPointsWithCorrespondence /
732  static_cast<float>(nLocalPoints / params.decimation_other_map_points);
733  extraResults.sumSqrDist = _sumSqrDist;
734 
735  MRPT_END
736 }
737 
738 /*---------------------------------------------------------------
739  isEmpty
740  ---------------------------------------------------------------*/
741 bool COccupancyGridMap2D::isEmpty() const { return m_is_empty; }
742 /*---------------------------------------------------------------
743  operator <
744  ---------------------------------------------------------------*/
748 {
749  return e1.first > e2.first;
750 }
751 
752 /*---------------------------------------------------------------
753  computePathCost
754  ---------------------------------------------------------------*/
756  float x1, float y1, float x2, float y2) const
757 {
758  float sumCost = 0;
759 
760  float dist = sqrt(square(x1 - x2) + square(y1 - y2));
761  int nSteps = round(1.5f * dist / resolution);
762 
763  for (int i = 0; i < nSteps; i++)
764  {
765  float x = x1 + (x2 - x1) * i / static_cast<float>(nSteps);
766  float y = y1 + (y2 - y1) * i / static_cast<float>(nSteps);
767  sumCost += getPos(x, y);
768  }
769 
770  if (nSteps)
771  return sumCost / static_cast<float>(nSteps);
772  else
773  return 0;
774 }
775 
777  const mrpt::maps::CMetricMap* otherMap,
778  const mrpt::poses::CPose3D& otherMapPose,
779  const TMatchingRatioParams& params) const
780 {
781  MRPT_UNUSED_PARAM(otherMap);
782  MRPT_UNUSED_PARAM(otherMapPose);
784  return 0;
785 }
static constexpr size_t size()
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:282
#define min(a, b)
float getResolution() const
Returns the resolution of the grid map.
mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
double x
X,Y coordinates.
double effectiveMappedArea
The target variable for the area of cells with information, i.e.
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:190
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
OccGridCellTraits::cellTypeUnsigned cellTypeUnsigned
static double H(double p)
Entropy computation internal function:
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::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.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
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:463
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:161
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:84
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:208
static CLogOddsGridMapLUT< COccupancyGridMap2D::cellType > logodd_lut
GLsizei const GLchar ** string
Definition: glext.h:4116
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...
#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.
const GLdouble * v
Definition: glext.h:3684
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
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:53
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:38
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:82
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:196
#define MRPT_END
Definition: exceptions.h:286
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.
float min_x
See COccupancyGridMap2D::COccupancyGridMap2D.
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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:
GLenum GLint GLint y
Definition: glext.h:3542
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:202
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.
GLuint res
Definition: glext.h:7385
GLenum GLint x
Definition: glext.h:3542
Lightweight 3D point.
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...
GLfloat GLfloat p
Definition: glext.h:6398
GLenum const GLfloat * params
Definition: glext.h:3538
int x2idx(float x) const
Transform a coordinate value into a cell index.
double phi
Orientation (rads)
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".
Definition: os.cpp:358
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
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:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019