MRPT  1.9.9
COccupancyGridMap2D_likelihood.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 
17 
18 using namespace mrpt;
19 using namespace mrpt::math;
20 using namespace mrpt::maps;
21 using namespace mrpt::obs;
22 using namespace mrpt::poses;
23 using namespace std;
24 
25 /*---------------------------------------------------------------
26  Computes the likelihood that a given observation was taken from a given pose in
27  the world being modeled with this map.
28  takenFrom The robot's pose the observation is supposed to be taken from.
29  obs The observation.
30  This method returns a likelihood in the range [0,1].
31  ---------------------------------------------------------------*/
32 double COccupancyGridMap2D::internal_computeObservationLikelihood(
33  const CObservation& obs, const CPose3D& takenFrom3D)
34 {
35  // Ignore laser scans if they are not planar or they are not
36  // at the altitude of this grid map:
38  {
39  const auto& scan = dynamic_cast<const CObservation2DRangeScan&>(obs);
40  if (!scan.isPlanarScan(insertionOptions.horizontalTolerance))
41  return -10;
42  if (insertionOptions.useMapAltitude &&
43  fabs(insertionOptions.mapAltitude - scan.sensorPose.z()) > 0.01)
44  return -10;
45 
46  // OK, go on...
47  }
48 
49  // Execute according to the selected method:
50  // --------------------------------------------
51  CPose2D takenFrom =
52  CPose2D(takenFrom3D); // 3D -> 2D, we are in a gridmap...
53 
54  switch (likelihoodOptions.likelihoodMethod)
55  {
56  default:
57  case lmRayTracing:
58  return computeObservationLikelihood_rayTracing(obs, takenFrom);
59 
60  case lmMeanInformation:
61  return computeObservationLikelihood_MI(obs, takenFrom);
62 
63  case lmConsensus:
64  return computeObservationLikelihood_Consensus(obs, takenFrom);
65 
66  case lmCellsDifference:
67  return computeObservationLikelihood_CellsDifference(obs, takenFrom);
68 
69  case lmLikelihoodField_Thrun:
70  return computeObservationLikelihood_likelihoodField_Thrun(
71  obs, takenFrom);
72 
73  case lmLikelihoodField_II:
74  return computeObservationLikelihood_likelihoodField_II(
75  obs, takenFrom);
76 
77  case lmConsensusOWA:
78  return computeObservationLikelihood_ConsensusOWA(obs, takenFrom);
79  };
80 }
81 
82 /*---------------------------------------------------------------
83  computeObservationLikelihood_Consensus
84 ---------------------------------------------------------------*/
85 double COccupancyGridMap2D::computeObservationLikelihood_Consensus(
86  const CObservation& obs, const CPose2D& takenFrom)
87 {
88  double likResult = 0;
89 
90  // This function depends on the observation type:
91  // -----------------------------------------------------
93  {
94  // THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan'
95  // classes only.");
96  return 1e-3;
97  }
98  // Observation is a laser range scan:
99  // -------------------------------------------
100  const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
101 
102  // Insert only HORIZONTAL scans, since the grid is supposed to
103  // be a horizontal representation of space.
104  if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
105  return 0.5f; // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!
106 
107  // Assure we have a 2D points-map representation of the points from the
108  // scan:
109  const auto* compareMap = o.buildAuxPointsMap<mrpt::maps::CPointsMap>();
110 
111  // Observation is a points map:
112  // -------------------------------------------
113  size_t Denom = 0;
114  // int Acells = 1;
115  TPoint2D pointGlobal, pointLocal;
116 
117  // Get the points buffers:
118 
119  // compareMap.getPointsBuffer( n, xs, ys, zs );
120  const size_t n = compareMap->size();
121 
122  for (size_t i = 0; i < n; i += likelihoodOptions.consensus_takeEachRange)
123  {
124  // Get the point and pass it to global coordinates:
125  compareMap->getPoint(i, pointLocal);
126  takenFrom.composePoint(pointLocal, pointGlobal);
127 
128  int cx0 = x2idx(pointGlobal.x);
129  int cy0 = y2idx(pointGlobal.y);
130 
131  likResult += 1 - getCell_nocheck(cx0, cy0);
132  Denom++;
133  }
134  if (Denom) likResult /= Denom;
135  likResult =
136  pow(likResult, static_cast<double>(likelihoodOptions.consensus_pow));
137 
138  return log(likResult);
139 }
140 
141 /*---------------------------------------------------------------
142  computeObservationLikelihood_ConsensusOWA
143 ---------------------------------------------------------------*/
144 double COccupancyGridMap2D::computeObservationLikelihood_ConsensusOWA(
145  const CObservation& obs, const CPose2D& takenFrom)
146 {
147  double likResult = 0;
148 
149  // This function depends on the observation type:
150  // -----------------------------------------------------
152  {
153  // THROW_EXCEPTION("This method is defined for 'CObservation2DRangeScan'
154  // classes only.");
155  return 1e-3;
156  }
157  // Observation is a laser range scan:
158  // -------------------------------------------
159  const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
160 
161  // Insert only HORIZONTAL scans, since the grid is supposed to
162  // be a horizontal representation of space.
163  if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
164  return 0.5; // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!
165 
166  // Assure we have a 2D points-map representation of the points from the
167  // scan:
169  insOpt.minDistBetweenLaserPoints = -1; // ALL the laser points
170 
171  const auto* compareMap =
172  o.buildAuxPointsMap<mrpt::maps::CPointsMap>(&insOpt);
173 
174  // Observation is a points map:
175  // -------------------------------------------
176  int Acells = 1;
177  TPoint2D pointGlobal, pointLocal;
178 
179  // Get the points buffers:
180  const size_t n = compareMap->size();
181 
182  // Store the likelihood values in this vector:
183  likelihoodOutputs.OWA_pairList.clear();
184  for (size_t i = 0; i < n; i++)
185  {
186  // Get the point and pass it to global coordinates:
187  compareMap->getPoint(i, pointLocal);
188  takenFrom.composePoint(pointLocal, pointGlobal);
189 
190  int cx0 = x2idx(pointGlobal.x);
191  int cy0 = y2idx(pointGlobal.y);
192 
193  int cxMin = max(0, cx0 - Acells);
194  int cxMax = min(static_cast<int>(size_x) - 1, cx0 + Acells);
195  int cyMin = max(0, cy0 - Acells);
196  int cyMax = min(static_cast<int>(size_y) - 1, cy0 + Acells);
197 
198  double lik = 0;
199 
200  for (int cx = cxMin; cx <= cxMax; cx++)
201  for (int cy = cyMin; cy <= cyMax; cy++)
202  lik += 1 - getCell_nocheck(cx, cy);
203 
204  int nCells = (cxMax - cxMin + 1) * (cyMax - cyMin + 1);
205  ASSERT_(nCells > 0);
206  lik /= nCells;
207 
208  TPairLikelihoodIndex element;
209  element.first = lik;
210  element.second = pointGlobal;
211  likelihoodOutputs.OWA_pairList.push_back(element);
212  } // for each range point
213 
214  // Sort the list of likelihood values, in descending order:
215  // ------------------------------------------------------------
216  std::sort(
217  likelihoodOutputs.OWA_pairList.begin(),
218  likelihoodOutputs.OWA_pairList.end());
219 
220  // Cut the vector to the highest "likelihoodOutputs.OWA_length" elements:
221  size_t M = likelihoodOptions.OWA_weights.size();
222  ASSERT_(likelihoodOutputs.OWA_pairList.size() >= M);
223 
224  likelihoodOutputs.OWA_pairList.resize(M);
225  likelihoodOutputs.OWA_individualLikValues.resize(M);
226  likResult = 0;
227  for (size_t k = 0; k < M; k++)
228  {
229  likelihoodOutputs.OWA_individualLikValues[k] =
230  likelihoodOutputs.OWA_pairList[k].first;
231  likResult += likelihoodOptions.OWA_weights[k] *
232  likelihoodOutputs.OWA_individualLikValues[k];
233  }
234 
235  return log(likResult);
236 }
237 
238 /*---------------------------------------------------------------
239  computeObservationLikelihood_CellsDifference
240 ---------------------------------------------------------------*/
241 double COccupancyGridMap2D::computeObservationLikelihood_CellsDifference(
242  const CObservation& obs, const CPose2D& takenFrom)
243 {
244  double ret = 0.5;
245 
246  // This function depends on the observation type:
247  // -----------------------------------------------------
249  {
250  // Observation is a laser range scan:
251  // -------------------------------------------
252  const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
253 
254  // Insert only HORIZONTAL scans, since the grid is supposed to
255  // be a horizontal representation of space.
256  if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
257  return 0.5; // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!
258 
259  // Build a copy of this occupancy grid:
260  COccupancyGridMap2D compareGrid(
261  takenFrom.x() - 10, takenFrom.x() + 10, takenFrom.y() - 10,
262  takenFrom.y() + 10, resolution);
263  CPose3D robotPose(takenFrom);
264  int Ax, Ay;
265 
266  // Insert in this temporary grid:
267  compareGrid.insertionOptions.maxDistanceInsertion =
268  insertionOptions.maxDistanceInsertion;
269  compareGrid.insertionOptions.maxOccupancyUpdateCertainty = 0.95f;
270  o.insertObservationInto(&compareGrid, &robotPose);
271 
272  // Save Cells offset between the two grids:
273  Ax = round((x_min - compareGrid.x_min) / resolution);
274  Ay = round((y_min - compareGrid.y_min) / resolution);
275 
276  int nCellsCompared = 0;
277  float cellsDifference = 0;
278  int x0 = max(0, Ax);
279  int y0 = max(0, Ay);
280  int x1 = min(compareGrid.size_x, size_x + Ax);
281  int y1 = min(compareGrid.size_y, size_y + Ay);
282 
283  for (int x = x0; x < x1; x += 1)
284  {
285  for (int y = y0; y < y1; y += 1)
286  {
287  float xx = compareGrid.idx2x(x);
288  float yy = compareGrid.idx2y(y);
289 
290  float c1 = getPos(xx, yy);
291  float c2 = compareGrid.getCell(x, y);
292  if (c2 < 0.45f || c2 > 0.55f)
293  {
294  nCellsCompared++;
295  if ((c1 > 0.5 && c2 < 0.5) || (c1 < 0.5 && c2 > 0.5))
296  cellsDifference++;
297  }
298  }
299  }
300  ret = 1 - cellsDifference / (nCellsCompared);
301  }
302  return log(ret);
303 }
304 
305 /*---------------------------------------------------------------
306  computeObservationLikelihood_MI
307 ---------------------------------------------------------------*/
308 double COccupancyGridMap2D::computeObservationLikelihood_MI(
309  const CObservation& obs, const CPose2D& takenFrom)
310 {
311  MRPT_START
312 
313  CPose3D poseRobot(takenFrom);
314  double res;
315 
316  // Dont modify the grid, only count the changes in Information
317  updateInfoChangeOnly.enabled = true;
318  insertionOptions.maxDistanceInsertion *=
319  likelihoodOptions.MI_ratio_max_distance;
320 
321  // Reset the new information counters:
322  updateInfoChangeOnly.cellsUpdated = 0;
323  updateInfoChangeOnly.I_change = 0;
324  updateInfoChangeOnly.laserRaysSkip = likelihoodOptions.MI_skip_rays;
325 
326  // Insert the observation (It will not be really inserted, only the
327  // information counted)
328  insertObservation(obs, &poseRobot);
329 
330  // Compute the change in I aported by the observation:
331  double newObservation_mean_I;
332  if (updateInfoChangeOnly.cellsUpdated)
333  newObservation_mean_I =
334  updateInfoChangeOnly.I_change / updateInfoChangeOnly.cellsUpdated;
335  else
336  newObservation_mean_I = 0;
337 
338  // Let the normal mode enabled, i.e. the grid can be updated
339  updateInfoChangeOnly.enabled = false;
340  insertionOptions.maxDistanceInsertion /=
341  likelihoodOptions.MI_ratio_max_distance;
342 
343  res =
344  pow(newObservation_mean_I,
345  static_cast<double>(likelihoodOptions.MI_exponent));
346 
347  return log(res);
348 
349  MRPT_END
350 }
351 
352 double COccupancyGridMap2D::computeObservationLikelihood_rayTracing(
353  const CObservation& obs, const CPose2D& takenFrom)
354 {
355  double ret = 0;
356 
357  // This function depends on the observation type:
358  // -----------------------------------------------------
360  {
361  // Observation is a laser range scan:
362  // -------------------------------------------
363  const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
364  CObservation2DRangeScan simulatedObs;
365 
366  // Insert only HORIZONTAL scans, since the grid is supposed to
367  // be a horizontal representation of space.
368  if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
369  return 0.5; // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!
370 
371  // The number of simulated rays will be original range scan rays /
372  // DOWNRATIO
373  int decimation = likelihoodOptions.rayTracing_decimation;
374  int nRays = o.getScanSize();
375 
376  // Perform simulation using same parameters than real observation:
377  simulatedObs.aperture = o.aperture;
378  simulatedObs.maxRange = o.maxRange;
379  simulatedObs.rightToLeft = o.rightToLeft;
380  simulatedObs.sensorPose = o.sensorPose;
381 
382  // Performs the scan simulation:
383  laserScanSimulator(
384  simulatedObs, // The in/out observation
385  takenFrom, // robot pose
386  0.45f, // Cells threshold
387  nRays, // Scan length
388  0, decimation);
389 
390  double stdLaser = likelihoodOptions.rayTracing_stdHit;
391  double stdSqrt2 = sqrt(2.0f) * stdLaser;
392 
393  // Compute likelihoods:
394  ret = 1;
395  // bool useDF = likelihoodOptions.rayTracing_useDistanceFilter;
396  float r_sim, r_obs;
397  double likelihood;
398 
399  for (int j = 0; j < nRays; j += decimation)
400  {
401  // Simulated and measured ranges:
402  r_sim = simulatedObs.getScanRange(j);
403  r_obs = o.getScanRange(j);
404 
405  // Is a valid range?
406  if (o.getScanRangeValidity(j))
407  {
408  likelihood =
409  0.1 / o.maxRange +
410  0.9 *
411  exp(-square(
412  min((float)fabs(r_sim - r_obs), 2.0f) / stdSqrt2));
413  ret += log(likelihood);
414  // printf("Sim=%f\tReal=%f\tlik=%f\n",r_sim,r_obs,likelihood);
415  }
416  }
417  }
418 
419  return ret;
420 }
421 /**/
422 
423 /*---------------------------------------------------------------
424  computeObservationLikelihood_likelihoodField_Thrun
425 ---------------------------------------------------------------*/
426 double COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_Thrun(
427  const CObservation& obs, const CPose2D& takenFrom)
428 {
429  MRPT_START
430 
431  double ret = 0;
432 
433  // This function depends on the observation type:
434  // -----------------------------------------------------
436  {
437  // Observation is a laser range scan:
438  // -------------------------------------------
439  const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
440 
441  // Insert only HORIZONTAL scans, since the grid is supposed to
442  // be a horizontal representation of space.
443  if (!o.isPlanarScan(insertionOptions.horizontalTolerance)) return -10;
444 
445  // Assure we have a 2D points-map representation of the points from the
446  // scan:
448  opts.minDistBetweenLaserPoints = resolution * 0.5f;
449  opts.isPlanarMap = true; // Already filtered above!
450  opts.horizontalTolerance = insertionOptions.horizontalTolerance;
451 
452  // Compute the likelihood of the points in this grid map:
453  ret = computeLikelihoodField_Thrun(
454  o.buildAuxPointsMap<mrpt::maps::CPointsMap>(&opts), &takenFrom);
455 
456  } // end of observation is a scan range 2D
457  else if (IS_CLASS(obs, CObservationRange))
458  {
459  // Sonar-like observations:
460  // ---------------------------------------
461  const auto& o = dynamic_cast<const CObservationRange&>(obs);
462 
463  // Create a point map representation of the observation:
464  CSimplePointsMap pts;
465  pts.insertionOptions.minDistBetweenLaserPoints = resolution * 0.5f;
466  pts.insertObservation(o);
467 
468  // Compute the likelihood of the points in this grid map:
469  ret = computeLikelihoodField_Thrun(&pts, &takenFrom);
470  }
471 
472  return ret;
473 
474  MRPT_END
475 }
476 
477 /*---------------------------------------------------------------
478  computeObservationLikelihood_likelihoodField_II
479 ---------------------------------------------------------------*/
480 double COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_II(
481  const CObservation& obs, const CPose2D& takenFrom)
482 {
483  MRPT_START
484 
485  double ret = 0;
486 
487  // This function depends on the observation type:
488  // -----------------------------------------------------
490  {
491  // Observation is a laser range scan:
492  // -------------------------------------------
493  const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
494 
495  // Insert only HORIZONTAL scans, since the grid is supposed to
496  // be a horizontal representation of space.
497  if (!o.isPlanarScan(insertionOptions.horizontalTolerance))
498  return 0.5f; // NO WAY TO ESTIMATE NON HORIZONTAL SCANS!!
499 
500  // Assure we have a 2D points-map representation of the points from the
501  // scan:
502 
503  // Compute the likelihood of the points in this grid map:
504  ret = computeLikelihoodField_II(
505  o.buildAuxPointsMap<mrpt::maps::CPointsMap>(), &takenFrom);
506 
507  } // end of observation is a scan range 2D
508 
509  return ret;
510 
511  MRPT_END
512 }
513 
514 /*---------------------------------------------------------------
515  computeLikelihoodField_Thrun
516  ---------------------------------------------------------------*/
517 double COccupancyGridMap2D::computeLikelihoodField_Thrun(
518  const CPointsMap* pm, const CPose2D* relativePose)
519 {
520  MRPT_START
521 
522  double ret;
523  size_t N = pm->size();
524  int K = (int)ceil(
525  likelihoodOptions.LF_maxCorrsDistance /*m*/ /
526  resolution); // The size of the checking area for matchings:
527 
528  bool Product_T_OrSum_F = !likelihoodOptions.LF_alternateAverageMethod;
529 
530  if (!N)
531  {
532  return -100; // No way to estimate this likelihood!!
533  }
534 
535  // Compute the likelihoods for each point:
536  ret = 0;
537 
538  float stdHit = likelihoodOptions.LF_stdHit;
539  float zHit = likelihoodOptions.LF_zHit;
540  float zRandom = likelihoodOptions.LF_zRandom;
541  float zRandomMaxRange = likelihoodOptions.LF_maxRange;
542  float zRandomTerm = zRandom / zRandomMaxRange;
543  float Q = -0.5f / square(stdHit);
544  int M = 0;
545 
546  unsigned int size_x_1 = size_x - 1;
547  unsigned int size_y_1 = size_y - 1;
548 
549 #define LIK_LF_CACHE_INVALID (66)
550 
551  // Aux. variables for the "for j" loop:
552  double thisLik = LIK_LF_CACHE_INVALID;
553  double maxCorrDist_sq = square(likelihoodOptions.LF_maxCorrsDistance);
554  double minimumLik = zRandomTerm + zHit * exp(Q * maxCorrDist_sq);
555  double ccos, ssin;
556 
557  if (likelihoodOptions.enableLikelihoodCache)
558  {
559  // Reset the precomputed likelihood values map
560  if (m_likelihoodCacheOutDated)
561  {
562  if (!map.empty())
563  precomputedLikelihood.assign(map.size(), LIK_LF_CACHE_INVALID);
564  else
565  precomputedLikelihood.clear();
566 
567  m_likelihoodCacheOutDated = false;
568  }
569  }
570 
571  cellType thresholdCellValue = p2l(0.5f);
572  int decimation = likelihoodOptions.LF_decimation;
573 
574  const double _resolution = this->resolution;
575  const double constDist2DiscrUnits = 100 / (_resolution * _resolution);
576  const double constDist2DiscrUnits_INV = 1.0 / constDist2DiscrUnits;
577 
578  if (N < 10) decimation = 1;
579 
580  TPoint2D pointLocal;
581  TPoint2D pointGlobal;
582 
583  for (size_t j = 0; j < N; j += decimation)
584  {
585  // Get the point and pass it to global coordinates:
586  if (relativePose)
587  {
588  pm->getPoint(j, pointLocal);
589 // pointGlobal = *relativePose + pointLocal;
590 #ifdef HAVE_SINCOS
591  ::sincos(relativePose->phi(), &ssin, &ccos);
592 #else
593  ccos = cos(relativePose->phi());
594  ssin = sin(relativePose->phi());
595 #endif
596  pointGlobal.x =
597  relativePose->x() + pointLocal.x * ccos - pointLocal.y * ssin;
598  pointGlobal.y =
599  relativePose->y() + pointLocal.x * ssin + pointLocal.y * ccos;
600  }
601  else
602  {
603  pm->getPoint(j, pointGlobal);
604  }
605 
606  // Point to cell indixes
607  int cx = x2idx(pointGlobal.x);
608  int cy = y2idx(pointGlobal.y);
609 
610  // Precomputed table:
611  // Tip: Comparison cx<0 is implicit in (unsigned)(x)>size...
612  if (static_cast<unsigned>(cx) >= size_x_1 ||
613  static_cast<unsigned>(cy) >= size_y_1)
614  {
615  // We are outside of the map: Assign the likelihood for the max.
616  // correspondence distance:
617  thisLik = minimumLik;
618  }
619  else
620  {
621  // We are into the map limits:
622  if (likelihoodOptions.enableLikelihoodCache)
623  {
624  thisLik = precomputedLikelihood[cx + cy * size_x];
625  }
626 
627  if (!likelihoodOptions.enableLikelihoodCache ||
628  thisLik == LIK_LF_CACHE_INVALID)
629  {
630  // Compute now:
631  // -------------
632  // Find the closest occupied cell in a certain range, given by
633  // K:
634  int xx1 = max(0, cx - K);
635  int xx2 = min(size_x_1, (unsigned)(cx + K));
636  int yy1 = max(0, cy - K);
637  int yy2 = min(size_y_1, (unsigned)(cy + K));
638 
639  // Optimized code: this part will be invoked a *lot* of times:
640  float occupiedMinDist;
641  {
642  cellType* mapPtr =
643  &map[xx1 + yy1 * size_x]; // Initial pointer position
644  unsigned incrAfterRow = size_x - ((xx2 - xx1) + 1);
645 
646  signed int Ax0 = 10 * (xx1 - cx);
647  signed int Ay = 10 * (yy1 - cy);
648 
649  unsigned int occupiedMinDistInt =
650  mrpt::round(maxCorrDist_sq * constDist2DiscrUnits);
651 
652  for (int yy = yy1; yy <= yy2; yy++)
653  {
654  unsigned int Ay2 =
655  square((unsigned int)(Ay)); // Square is faster
656  // with unsigned.
657  signed short Ax = Ax0;
658  cellType cell;
659 
660  for (int xx = xx1; xx <= xx2; xx++)
661  {
662  if ((cell = *mapPtr++) < thresholdCellValue)
663  {
664  unsigned int d =
665  square((unsigned int)(Ax)) + Ay2;
666  keep_min(occupiedMinDistInt, d);
667  }
668  Ax += 10;
669  }
670  // Go to (xx1,yy++)
671  mapPtr += incrAfterRow;
672  Ay += 10;
673  }
674 
675  occupiedMinDist =
676  occupiedMinDistInt * constDist2DiscrUnits_INV;
677  }
678 
679  if (likelihoodOptions.LF_useSquareDist)
680  occupiedMinDist *= occupiedMinDist;
681 
682  thisLik = zRandomTerm + zHit * exp(Q * occupiedMinDist);
683 
684  if (likelihoodOptions.enableLikelihoodCache)
685  // And save it into the table and into "thisLik":
686  precomputedLikelihood[cx + cy * size_x] = thisLik;
687  }
688  }
689 
690  // Update the likelihood:
691  if (Product_T_OrSum_F)
692  {
693  ret += log(thisLik);
694  }
695  else
696  {
697  ret += thisLik;
698  M++;
699  }
700  } // end of for each point in the scan
701 
702  if (!Product_T_OrSum_F) ret = log(ret / M);
703 
704  return ret;
705 
706  MRPT_END
707 }
708 
709 /*---------------------------------------------------------------
710  computeLikelihoodField_II
711  ---------------------------------------------------------------*/
712 double COccupancyGridMap2D::computeLikelihoodField_II(
713  const CPointsMap* pm, const CPose2D* relativePose)
714 {
715  MRPT_START
716 
717  double ret;
718  size_t N = pm->size();
719 
720  if (!N) return 1e-100; // No way to estimate this likelihood!!
721 
722  // Compute the likelihoods for each point:
723  ret = 0;
724  // if (likelihoodOptions.LF_alternateAverageMethod)
725  // ret = 0;
726  // else ret = 1;
727 
728  TPoint2D pointLocal, pointGlobal;
729 
730  float zRandomTerm = 1.0f / likelihoodOptions.LF_maxRange;
731  float Q = -0.5f / square(likelihoodOptions.LF_stdHit);
732 
733  // Aux. cell indixes variables:
734  int cx, cy;
735  size_t j;
736  int cx0, cy0;
737  int cx_min, cx_max;
738  int cy_min, cy_max;
739  int maxRangeInCells =
740  (int)ceil(likelihoodOptions.LF_maxCorrsDistance / resolution);
741  int nCells = 0;
742 
743  // -----------------------------------------------------
744  // Compute around a window of neigbors around each point
745  // -----------------------------------------------------
746  for (j = 0; j < N; j += likelihoodOptions.LF_decimation)
747  {
748  // Get the point and pass it to global coordinates:
749  // ---------------------------------------------
750  if (relativePose)
751  {
752  pm->getPoint(j, pointLocal);
753  pointGlobal = *relativePose + pointLocal;
754  }
755  else
756  {
757  pm->getPoint(j, pointGlobal);
758  }
759 
760  // Point to cell indixes:
761  // ---------------------------------------------
762  cx0 = x2idx(pointGlobal.x);
763  cy0 = y2idx(pointGlobal.y);
764 
765  // Compute the range of cells to compute:
766  // ---------------------------------------------
767  cx_min = max(cx0 - maxRangeInCells, 0);
768  cx_max = min(cx0 + maxRangeInCells, static_cast<int>(size_x));
769  cy_min = max(cy0 - maxRangeInCells, 0);
770  cy_max = min(cy0 + maxRangeInCells, static_cast<int>(size_y));
771 
772  // debugImg.rectangle(cx_min,cy_min,cx_max,cy_max,0xFF0000 );
773 
774  // Compute over the window of cells:
775  // ---------------------------------------------
776  double lik = 0;
777  for (cx = cx_min; cx <= cx_max; cx++)
778  {
779  for (cy = cy_min; cy <= cy_max; cy++)
780  {
781  float P_free = getCell(cx, cy);
782  float termDist =
783  exp(Q * (square(idx2x(cx) - pointGlobal.x) +
784  square(idx2y(cy) - pointGlobal.y)));
785 
786  lik += P_free * zRandomTerm + (1 - P_free) * termDist;
787  } // end for cy
788  } // end for cx
789 
790  // Update the likelihood:
791  if (likelihoodOptions.LF_alternateAverageMethod)
792  ret += lik;
793  else
794  ret += log(lik / ((cy_max - cy_min + 1) * (cx_max - cx_min + 1)));
795  nCells++;
796 
797  } // end of for each point in the scan
798 
799  if (likelihoodOptions.LF_alternateAverageMethod && nCells > 0)
800  ret = log(ret / nCells);
801  else
802  ret = ret / nCells;
803 
804  return ret;
805 
806  MRPT_END
807 }
808 
809 /*---------------------------------------------------------------
810  Initilization of values, don't needed to be called directly.
811  ---------------------------------------------------------------*/
812 COccupancyGridMap2D::TLikelihoodOptions::TLikelihoodOptions()
813  : OWA_weights(100, 1 / 100.0f)
814 
815 {
816 }
817 
818 /*---------------------------------------------------------------
819  loadFromConfigFile
820  ---------------------------------------------------------------*/
822  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
823 {
824  likelihoodMethod =
825  iniFile.read_enum(section, "likelihoodMethod", likelihoodMethod);
826 
827  enableLikelihoodCache = iniFile.read_bool(
828  section, "enableLikelihoodCache", enableLikelihoodCache);
829 
830  LF_stdHit = iniFile.read_float(section, "LF_stdHit", LF_stdHit);
831  LF_zHit = iniFile.read_float(section, "LF_zHit", LF_zHit);
832  LF_zRandom = iniFile.read_float(section, "LF_zRandom", LF_zRandom);
833  LF_maxRange = iniFile.read_float(section, "LF_maxRange", LF_maxRange);
834  LF_decimation = iniFile.read_int(section, "LF_decimation", LF_decimation);
835  LF_maxCorrsDistance =
836  iniFile.read_float(section, "LF_maxCorrsDistance", LF_maxCorrsDistance);
837  LF_useSquareDist =
838  iniFile.read_bool(section, "LF_useSquareDist", LF_useSquareDist);
839  LF_alternateAverageMethod = iniFile.read_bool(
840  section, "LF_alternateAverageMethod", LF_alternateAverageMethod);
841 
842  MI_exponent = iniFile.read_float(section, "MI_exponent", MI_exponent);
843  MI_skip_rays = iniFile.read_int(section, "MI_skip_rays", MI_skip_rays);
844  MI_ratio_max_distance = iniFile.read_float(
845  section, "MI_ratio_max_distance", MI_ratio_max_distance);
846 
847  rayTracing_useDistanceFilter = iniFile.read_bool(
848  section, "rayTracing_useDistanceFilter", rayTracing_useDistanceFilter);
849  rayTracing_stdHit =
850  iniFile.read_float(section, "rayTracing_stdHit", rayTracing_stdHit);
851 
852  consensus_takeEachRange = iniFile.read_int(
853  section, "consensus_takeEachRange", consensus_takeEachRange);
854  consensus_pow = iniFile.read_float(section, "consensus_pow", consensus_pow);
855 
856  iniFile.read_vector(section, "OWA_weights", OWA_weights, OWA_weights);
857 }
858 
859 /*---------------------------------------------------------------
860  dumpToTextStream
861  ---------------------------------------------------------------*/
863  std::ostream& out) const
864 {
865  out << "\n----------- [COccupancyGridMap2D::TLikelihoodOptions] "
866  "------------ "
867  "\n\n";
868 
869  out << "likelihoodMethod = ";
870  switch (likelihoodMethod)
871  {
872  case lmMeanInformation:
873  out << "lmMeanInformation";
874  break;
875  case lmRayTracing:
876  out << "lmRayTracing";
877  break;
878  case lmConsensus:
879  out << "lmConsensus";
880  break;
881  case lmCellsDifference:
882  out << "lmCellsDifference";
883  break;
885  out << "lmLikelihoodField_Thrun";
886  break;
888  out << "lmLikelihoodField_II";
889  break;
890  case lmConsensusOWA:
891  out << "lmConsensusOWA";
892  break;
893  default:
894  out << "UNKNOWN!!!";
895  break;
896  }
897  out << "\n";
898 
899  out << mrpt::format(
900  "enableLikelihoodCache = %c\n",
901  enableLikelihoodCache ? 'Y' : 'N');
902 
903  out << mrpt::format(
904  "LF_stdHit = %f\n", LF_stdHit);
905  out << mrpt::format(
906  "LF_zHit = %f\n", LF_zHit);
907  out << mrpt::format(
908  "LF_zRandom = %f\n", LF_zRandom);
909  out << mrpt::format(
910  "LF_maxRange = %f\n", LF_maxRange);
911  out << mrpt::format(
912  "LF_decimation = %u\n", LF_decimation);
913  out << mrpt::format(
914  "LF_maxCorrsDistance = %f\n", LF_maxCorrsDistance);
915  out << mrpt::format(
916  "LF_useSquareDist = %c\n",
917  LF_useSquareDist ? 'Y' : 'N');
918  out << mrpt::format(
919  "LF_alternateAverageMethod = %c\n",
920  LF_alternateAverageMethod ? 'Y' : 'N');
921  out << mrpt::format(
922  "MI_exponent = %f\n", MI_exponent);
923  out << mrpt::format(
924  "MI_skip_rays = %u\n", MI_skip_rays);
925  out << mrpt::format(
926  "MI_ratio_max_distance = %f\n",
927  MI_ratio_max_distance);
928  out << mrpt::format(
929  "rayTracing_useDistanceFilter = %c\n",
930  rayTracing_useDistanceFilter ? 'Y' : 'N');
931  out << mrpt::format(
932  "rayTracing_decimation = %u\n",
933  rayTracing_decimation);
934  out << mrpt::format(
935  "rayTracing_stdHit = %f\n", rayTracing_stdHit);
936  out << mrpt::format(
937  "consensus_takeEachRange = %u\n",
938  consensus_takeEachRange);
939  out << mrpt::format(
940  "consensus_pow = %.02f\n", consensus_pow);
941  out << "OWA_weights = [";
942  for (size_t i = 0; i < OWA_weights.size(); i++)
943  {
944  if (i < 3 || i > (OWA_weights.size() - 3))
945  out << mrpt::format("%.03f ", OWA_weights[i]);
946  else if (i == 3 && OWA_weights.size() > 6)
947  out << " ... ";
948  }
949  out << mrpt::format("] (size=%u)\n", (unsigned)OWA_weights.size());
950  out << "\n";
951 }
952 
953 /** Returns true if this map is able to compute a sensible likelihood function
954  * for this observation (i.e. an occupancy grid map cannot with an image).
955  * \param obs The observation.
956  * \sa computeObservationLikelihood
957  */
959  const mrpt::obs::CObservation& obs) const
960 {
961  // Ignore laser scans if they are not planar or they are not
962  // at the altitude of this grid map:
964  {
965  const auto& scan = dynamic_cast<const CObservation2DRangeScan&>(obs);
966 
967  if (!scan.isPlanarScan(insertionOptions.horizontalTolerance))
968  return false;
970  fabs(insertionOptions.mapAltitude - scan.sensorPose.z()) > 0.01)
971  return false;
972 
973  // OK, go on...
974  return true;
975  }
976  else // Is not a laser scanner...
977  {
978  return false;
979  }
980 }
double x
X,Y coordinates.
Definition: TPoint2D.h:23
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value...
void getPoint(size_t index, float &x, float &y, float &z) const
Access to a given point from map, as a 2D point.
Definition: CPointsMap.cpp:194
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Definition: CPointsMap.h:253
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
#define MRPT_START
Definition: exceptions.h:241
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
const POINTSMAP * buildAuxPointsMap(const void *options=nullptr) const
Returns a cached points map representing this laser scan, building it upon the first call...
GLenum GLsizei n
Definition: glext.h:5136
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
TInsertionOptions insertionOptions
With this struct options are provided to the observation insertion process.
void composePoint(double lx, double ly, double &gx, double &gy) const
An alternative, slightly more efficient way of doing with G and L being 2D points and P this 2D pose...
Definition: CPose2D.cpp:199
size_t getScanSize() const
Get number of scan rays.
bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation &obs) const override
Returns true if this map is able to compute a sensible likelihood function for this observation (i...
float maxRange
The maximum range allowed by the device, in meters (e.g.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:221
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
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.
T square(const T x)
Inline function for the square of a number.
std::pair< double, mrpt::math::TPoint2D > TPairLikelihoodIndex
Auxiliary private class.
#define LIK_LF_CACHE_INVALID
This namespace contains representation of robot actions and observations.
string iniFile(myDataDir+string("benchmark-options.ini"))
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini" file.
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
A class for storing an occupancy grid map.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
constexpr std::size_t size() const
Definition: TPoseOrPoint.h:58
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
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, then processed by calls to this class (default=0).
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
const float & getScanRange(const size_t i) const
The range values of the scan, in meters.
#define MRPT_END
Definition: exceptions.h:245
bool useMapAltitude
The parameter "mapAltitude" has effect while inserting observations in the grid only if this is true...
float horizontalTolerance
The tolerance in rads in pitch & roll for a laser scan to be considered horizontal, considered only when isPlanarMap=true (default=0).
Definition: CPointsMap.h:257
float aperture
The "aperture" or field-of-view of the range finder, in radians (typically M_PI = 180 degrees)...
float mapAltitude
The altitude (z-axis) of 2D scans (within a 0.01m tolerance) for they to be inserted in this map! ...
OccGridCellTraits::cellType cellType
The type of the map cells:
GLenum GLint GLint y
Definition: glext.h:3542
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:273
GLuint res
Definition: glext.h:7385
GLenum GLint x
Definition: glext.h:3542
Lightweight 2D point.
Definition: TPoint2D.h:31
size_t size() const
Save the point cloud as a PCL PCD file, in either ASCII or binary format.
Definition: CPointsMap.h:441
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot at the moment of starting the scan.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:93
bool rightToLeft
The scanning direction: true=counterclockwise; false=clockwise.
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:234
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: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019