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  "mrpt::maps::COccupancyGridMap2D,occupancyGrid",
33 
34 COccupancyGridMap2D::TMapDefinition::TMapDefinition() = default;
35 
36 void COccupancyGridMap2D::TMapDefinition::loadFromConfigFile_map_specific(
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  unsigned long i;
355  float h, p;
356 
357 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
358  unsigned int N = 256;
359 #else
360  unsigned int N = 65536;
361 #endif
362 
363  // Compute the entropy table: The entropy for each posible cell value
364  // ----------------------------------------------------------------------
365  if (entropyTable.size() != N)
366  {
367  entropyTable.resize(N, 0);
368  for (i = 0; i < N; i++)
369  {
370  p = l2p(static_cast<cellType>(i));
371  h = H(p) + H(1 - p);
372 
373  // Cell's probabilities rounding problem fixing:
374  if (i == 0 || i == (N - 1)) h = 0;
375  if (h > (MAX_H - 1e-4)) h = MAX_H;
376 
377  entropyTable[i] = h;
378  }
379  }
380 
381  // Initialize the global results:
382  info.H = info.I = 0;
383  info.effectiveMappedCells = 0;
384 
385  info.H = info.I = 0;
386  info.effectiveMappedCells = 0;
387  for (signed char it : map)
388  {
389  auto ctu = static_cast<cellTypeUnsigned>(it);
390  h = entropyTable[ctu];
391  info.H += h;
392  if (h < (MAX_H - 0.001f))
393  {
394  info.effectiveMappedCells++;
395  info.I -= h;
396  }
397  }
398 
399  // The info: (See ref. paper EMMI in IROS 2006)
400  info.I /= MAX_H;
401  info.I += info.effectiveMappedCells;
402 
403  // Mean values:
404  // ------------------------------------------
405  info.effectiveMappedArea =
407  if (info.effectiveMappedCells)
408  {
409  info.mean_H = info.H / info.effectiveMappedCells;
410  info.mean_I = info.I / info.effectiveMappedCells;
411  }
412  else
413  {
414  info.mean_H = 0;
415  info.mean_I = 0;
416  }
417 }
418 
419 /*---------------------------------------------------------------
420  Entropy aux. function
421  ---------------------------------------------------------------*/
422 double COccupancyGridMap2D::H(double p)
423 {
424  if (p == 0 || p == 1)
425  return 0;
426  else
427  return -p * log(p);
428 }
429 
430 /*---------------------------------------------------------------
431  clear
432  ---------------------------------------------------------------*/
434 {
435  setSize(-10, 10, -10, 10, getResolution());
436  // resetFeaturesCache();
437  // For the precomputed likelihood trick:
439 }
440 
441 /*---------------------------------------------------------------
442  fill
443  ---------------------------------------------------------------*/
444 void COccupancyGridMap2D::fill(float default_value)
445 {
446  cellType defValue = p2l(default_value);
447  for (auto it = map.begin(); it < map.end(); ++it) *it = defValue;
448  // For the precomputed likelihood trick:
450  // resetFeaturesCache();
451 }
452 
453 /*---------------------------------------------------------------
454  updateCell
455  ---------------------------------------------------------------*/
456 void COccupancyGridMap2D::updateCell(int x, int y, float v)
457 {
458  // Tip: if x<0, (unsigned)(x) will also be >>> size_x ;-)
459  if (static_cast<unsigned int>(x) >= size_x ||
460  static_cast<unsigned int>(y) >= size_y)
461  return;
462 
463  // Get the current contents of the cell:
464  cellType& theCell = map[x + y * size_x];
465 
466  // Compute the new Bayesian-fused value of the cell:
468  {
469  float old = l2p(theCell);
470  float new_v = 1 / (1 + (1 - v) * (1 - old) / (old * v));
472  updateInfoChangeOnly.I_change += 1 - (H(new_v) + H(1 - new_v)) / MAX_H;
473  }
474  else
475  {
476  cellType obs =
477  p2l(v); // The observation: will be >0 for free, <0 for occupied.
478  if (obs > 0)
479  {
480  if (theCell > (OCCGRID_CELLTYPE_MAX - obs))
481  theCell = OCCGRID_CELLTYPE_MAX; // Saturate
482  else
483  theCell += obs;
484  }
485  else
486  {
487  if (theCell < (OCCGRID_CELLTYPE_MIN - obs))
488  theCell = OCCGRID_CELLTYPE_MIN; // Saturate
489  else
490  theCell += obs;
491  }
492  }
493 }
494 
495 /*---------------------------------------------------------------
496  subSample
497  ---------------------------------------------------------------*/
499 {
500  std::vector<cellType> newMap;
501 
502  ASSERT_(downRatio > 0);
503 
504  resolution *= downRatio;
505 
506  int newSizeX = round((x_max - x_min) / resolution);
507  int newSizeY = round((y_max - y_min) / resolution);
508 
509  newMap.resize(newSizeX * newSizeY);
510 
511  for (int x = 0; x < newSizeX; x++)
512  {
513  for (int y = 0; y < newSizeY; y++)
514  {
515  float newCell = 0;
516 
517  for (int xx = 0; xx < downRatio; xx++)
518  for (int yy = 0; yy < downRatio; yy++)
519  newCell += getCell(x * downRatio + xx, y * downRatio + yy);
520 
521  newCell /= (downRatio * downRatio);
522 
523  newMap[x + y * newSizeX] = p2l(newCell);
524  }
525  }
526 
528  map = newMap;
529 }
530 
531 /*---------------------------------------------------------------
532  computeMatchingWith
533  ---------------------------------------------------------------*/
535  const mrpt::maps::CMetricMap* otherMap2, const CPose2D& otherMapPose_,
536  TMatchingPairList& correspondences, const TMatchingParams& params,
537  TMatchingExtraResults& extraResults) const
538 {
539  MRPT_START
540 
541  extraResults = TMatchingExtraResults();
542 
543  ASSERT_ABOVE_(params.decimation_other_map_points, 0);
545  params.offset_other_map_points, params.decimation_other_map_points);
546 
548  const auto* otherMap = static_cast<const CPointsMap*>(otherMap2);
549 
550  const TPose2D otherMapPose = otherMapPose_.asTPose();
551 
552  const size_t nLocalPoints = otherMap->size();
553  std::vector<float> x_locals(nLocalPoints), y_locals(nLocalPoints),
554  z_locals(nLocalPoints);
555 
556  const float sin_phi = sin(otherMapPose.phi);
557  const float cos_phi = cos(otherMapPose.phi);
558 
559  size_t nOtherMapPointsWithCorrespondence =
560  0; // Number of points with one corrs. at least
561  size_t nTotalCorrespondences = 0; // Total number of corrs
562  float _sumSqrDist = 0;
563 
564  // The number of cells to look around each point:
565  const int cellsSearchRange =
566  round(params.maxDistForCorrespondence / resolution);
567 
568  // Initially there are no correspondences:
569  correspondences.clear();
570 
571  // Hay mapa local?
572  if (!nLocalPoints) return; // No
573 
574  // Solo hacer matching si existe alguna posibilidad de que
575  // los dos mapas se toquen:
576  // -----------------------------------------------------------
577  float local_x_min = std::numeric_limits<float>::max();
578  float local_x_max = -std::numeric_limits<float>::max();
579  float local_y_min = std::numeric_limits<float>::max();
580  float local_y_max = -std::numeric_limits<float>::max();
581 
582  const auto& otherMap_pxs = otherMap->getPointsBufferRef_x();
583  const auto& otherMap_pys = otherMap->getPointsBufferRef_y();
584  const auto& otherMap_pzs = otherMap->getPointsBufferRef_z();
585 
586  // Translate all local map points:
587  for (unsigned int localIdx = params.offset_other_map_points;
588  localIdx < nLocalPoints;
589  localIdx += params.decimation_other_map_points)
590  {
591  // Girar y desplazar cada uno de los puntos del local map:
592  const float xx = x_locals[localIdx] = otherMapPose.x +
593  cos_phi * otherMap_pxs[localIdx] -
594  sin_phi * otherMap_pys[localIdx];
595  const float yy = y_locals[localIdx] = otherMapPose.y +
596  sin_phi * otherMap_pxs[localIdx] +
597  cos_phi * otherMap_pys[localIdx];
598  z_locals[localIdx] = /* otherMapPose.z +*/ otherMap_pzs[localIdx];
599 
600  // mantener el max/min de los puntos:
601  local_x_min = min(local_x_min, xx);
602  local_x_max = max(local_x_max, xx);
603  local_y_min = min(local_y_min, yy);
604  local_y_max = max(local_y_max, yy);
605  }
606 
607  // If the local map is entirely out of the grid,
608  // do not even try to match them!!
609  if (local_x_min > x_max || local_x_max < x_min || local_y_min > y_max ||
610  local_y_max < y_min)
611  return; // Matching is NULL!
612 
613  const cellType thresholdCellValue = p2l(0.5f);
614 
615  // For each point in the other map:
616  for (unsigned int localIdx = params.offset_other_map_points;
617  localIdx < nLocalPoints;
618  localIdx += params.decimation_other_map_points)
619  {
620  // Starting value:
621  float maxDistForCorrespondenceSquared =
622  square(params.maxDistForCorrespondence);
623 
624  // For speed-up:
625  const float x_local = x_locals[localIdx];
626  const float y_local = y_locals[localIdx];
627  const float z_local = z_locals[localIdx];
628 
629  // Look for the occupied cell closest from the map point:
630  float min_dist = 1e6;
631  TMatchingPair closestCorr;
632 
633  // Get the indexes of cell where the point falls:
634  const int cx0 = x2idx(x_local);
635  const int cy0 = y2idx(y_local);
636 
637  // Get the rectangle to look for into:
638  const int cx_min = max(0, cx0 - cellsSearchRange);
639  const int cx_max =
640  min(static_cast<int>(size_x) - 1, cx0 + cellsSearchRange);
641  const int cy_min = max(0, cy0 - cellsSearchRange);
642  const int cy_max =
643  min(static_cast<int>(size_y) - 1, cy0 + cellsSearchRange);
644 
645  // Will be set to true if a corrs. is found:
646  bool thisLocalHasCorr = false;
647 
648  // Look in nearby cells:
649  for (int cx = cx_min; cx <= cx_max; cx++)
650  {
651  for (int cy = cy_min; cy <= cy_max; cy++)
652  {
653  // Is an occupied cell?
654  if (map[cx + cy * size_x] <
655  thresholdCellValue) // getCell(cx,cy)<0.49)
656  {
657  const float residual_x = idx2x(cx) - x_local;
658  const float residual_y = idx2y(cy) - y_local;
659 
660  // Compute max. allowed distance:
661  maxDistForCorrespondenceSquared = square(
662  params.maxAngularDistForCorrespondence *
663  params.angularDistPivotPoint.distanceTo(
664  TPoint3D(x_local, y_local, 0)) +
665  params.maxDistForCorrespondence);
666 
667  // Square distance to the point:
668  const float this_dist =
669  square(residual_x) + square(residual_y);
670 
671  if (this_dist < maxDistForCorrespondenceSquared)
672  {
673  if (!params.onlyKeepTheClosest)
674  {
675  // save the correspondence:
676  nTotalCorrespondences++;
677  TMatchingPair mp;
678  mp.this_idx = cx + cy * size_x;
679  mp.this_x = idx2x(cx);
680  mp.this_y = idx2y(cy);
681  mp.this_z = z_local;
682  mp.other_idx = localIdx;
683  mp.other_x = otherMap_pxs[localIdx];
684  mp.other_y = otherMap_pys[localIdx];
685  mp.other_z = otherMap_pzs[localIdx];
686  correspondences.push_back(mp);
687  }
688  else
689  {
690  // save the closest only:
691  if (this_dist < min_dist)
692  {
693  min_dist = this_dist;
694 
695  closestCorr.this_idx = cx + cy * size_x;
696  closestCorr.this_x = idx2x(cx);
697  closestCorr.this_y = idx2y(cy);
698  closestCorr.this_z = z_local;
699  closestCorr.other_idx = localIdx;
700  closestCorr.other_x = otherMap_pxs[localIdx];
701  closestCorr.other_y = otherMap_pys[localIdx];
702  closestCorr.other_z = otherMap_pzs[localIdx];
703  }
704  }
705 
706  // At least one:
707  thisLocalHasCorr = true;
708  }
709  }
710  }
711  } // End of find closest nearby cell
712 
713  // save the closest correspondence:
714  if (params.onlyKeepTheClosest &&
715  (min_dist < maxDistForCorrespondenceSquared))
716  {
717  nTotalCorrespondences++;
718  correspondences.push_back(closestCorr);
719  }
720 
721  // At least one corr:
722  if (thisLocalHasCorr)
723  {
724  nOtherMapPointsWithCorrespondence++;
725 
726  // Accumulate the MSE:
727  _sumSqrDist += min_dist;
728  }
729 
730  } // End "for each local point"...
731 
732  extraResults.correspondencesRatio =
733  nOtherMapPointsWithCorrespondence /
734  static_cast<float>(nLocalPoints / params.decimation_other_map_points);
735  extraResults.sumSqrDist = _sumSqrDist;
736 
737  MRPT_END
738 }
739 
740 /*---------------------------------------------------------------
741  isEmpty
742  ---------------------------------------------------------------*/
743 bool COccupancyGridMap2D::isEmpty() const { return m_is_empty; }
744 /*---------------------------------------------------------------
745  operator <
746  ---------------------------------------------------------------*/
750 {
751  return e1.first > e2.first;
752 }
753 
754 /*---------------------------------------------------------------
755  computePathCost
756  ---------------------------------------------------------------*/
758  float x1, float y1, float x2, float y2) const
759 {
760  float sumCost = 0;
761 
762  float dist = sqrt(square(x1 - x2) + square(y1 - y2));
763  int nSteps = round(1.5f * dist / resolution);
764 
765  for (int i = 0; i < nSteps; i++)
766  {
767  float x = x1 + (x2 - x1) * i / static_cast<float>(nSteps);
768  float y = y1 + (y2 - y1) * i / static_cast<float>(nSteps);
769  sumCost += getPos(x, y);
770  }
771 
772  if (nSteps)
773  return sumCost / static_cast<float>(nSteps);
774  else
775  return 0;
776 }
777 
779  const mrpt::maps::CMetricMap* otherMap,
780  const mrpt::poses::CPose3D& otherMapPose,
781  const TMatchingRatioParams& params) const
782 {
783  MRPT_UNUSED_PARAM(otherMap);
784  MRPT_UNUSED_PARAM(otherMapPose);
786  return 0;
787 }
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
Parameters for CMetricMap::compute3DMatchingRatio()
static constexpr cellType OCCGRID_CELLTYPE_MIN
Discrete to float conversion factors: The min/max values of the integer cell type, eg.
void clear()
Erase the contents of all the cells.
Definition: CDynamicGrid.h:101
#define MRPT_START
Definition: exceptions.h:241
float getResolution() const
Returns the resolution of the grid map.
mrpt::maps::COccupancyGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
double x
X,Y coordinates.
Definition: TPose2D.h:30
double effectiveMappedArea
The target variable for the area of cells with information, i.e.
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
OccGridCellTraits::cellTypeUnsigned cellTypeUnsigned
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
#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:102
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
T square(const T x)
Inline function for the square of a number.
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:85
mrpt::vision::TStereoCalibResults out
int cellsUpdated
The cummulative updated cells count: This is updated only from the "updateCell" method.
static std::vector< float > entropyTable
Internally used to speed-up entropy calculation.
#define ASSERT_ABOVE_(__A, __B)
Definition: exceptions.h:155
#define MRPT_END
Definition: exceptions.h:245
bool m_is_empty
True upon construction; used by isEmpty()
void internal_clear() override
Clear the map: It set all cells to their default occupancy value (0.5), without changing the resoluti...
mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts
Probabilistic observation likelihood options.
Lightweight 2D pose.
Definition: TPose2D.h:22
float min_x
See COccupancyGridMap2D::COccupancyGridMap2D.
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:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: cfb2df9d3 Fri Nov 15 06:57:14 2019 +0100 at vie nov 15 07:00:11 CET 2019