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