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