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