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 = static_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 = static_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 = static_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 = static_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 = static_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->scan.size();
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.scan[j];
403  r_obs = o->scan[j];
404 
405  // Is a valid range?
406  if (o->validRange[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 = static_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 = static_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 = static_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  float occupiedMinDist;
557 
558  if (likelihoodOptions.enableLikelihoodCache)
559  {
560  // Reset the precomputed likelihood values map
561  if (m_likelihoodCacheOutDated)
562  {
563  if (!map.empty())
564  precomputedLikelihood.assign(map.size(), LIK_LF_CACHE_INVALID);
565  else
566  precomputedLikelihood.clear();
567 
568  m_likelihoodCacheOutDated = false;
569  }
570  }
571 
572  cellType thresholdCellValue = p2l(0.5f);
573  int decimation = likelihoodOptions.LF_decimation;
574 
575  const double _resolution = this->resolution;
576  const double constDist2DiscrUnits = 100 / (_resolution * _resolution);
577  const double constDist2DiscrUnits_INV = 1.0 / constDist2DiscrUnits;
578 
579  if (N < 10) decimation = 1;
580 
581  TPoint2D pointLocal;
582  TPoint2D pointGlobal;
583 
584  for (size_t j = 0; j < N; j += decimation)
585  {
586  occupiedMinDist = maxCorrDist_sq; // The max.distance
587 
588  // Get the point and pass it to global coordinates:
589  if (relativePose)
590  {
591  pm->getPoint(j, pointLocal);
592 // pointGlobal = *relativePose + pointLocal;
593 #ifdef HAVE_SINCOS
594  ::sincos(relativePose->phi(), &ssin, &ccos);
595 #else
596  ccos = cos(relativePose->phi());
597  ssin = sin(relativePose->phi());
598 #endif
599  pointGlobal.x =
600  relativePose->x() + pointLocal.x * ccos - pointLocal.y * ssin;
601  pointGlobal.y =
602  relativePose->y() + pointLocal.x * ssin + pointLocal.y * ccos;
603  }
604  else
605  {
606  pm->getPoint(j, pointGlobal);
607  }
608 
609  // Point to cell indixes
610  int cx = x2idx(pointGlobal.x);
611  int cy = y2idx(pointGlobal.y);
612 
613  // Precomputed table:
614  // Tip: Comparison cx<0 is implicit in (unsigned)(x)>size...
615  if (static_cast<unsigned>(cx) >= size_x_1 ||
616  static_cast<unsigned>(cy) >= size_y_1)
617  {
618  // We are outside of the map: Assign the likelihood for the max.
619  // correspondence distance:
620  thisLik = minimumLik;
621  }
622  else
623  {
624  // We are into the map limits:
625  if (likelihoodOptions.enableLikelihoodCache)
626  {
627  thisLik = precomputedLikelihood[cx + cy * size_x];
628  }
629 
630  if (!likelihoodOptions.enableLikelihoodCache ||
631  thisLik == LIK_LF_CACHE_INVALID)
632  {
633  // Compute now:
634  // -------------
635  // Find the closest occupied cell in a certain range, given by
636  // K:
637  int xx1 = max(0, cx - K);
638  int xx2 = min(size_x_1, (unsigned)(cx + K));
639  int yy1 = max(0, cy - K);
640  int yy2 = min(size_y_1, (unsigned)(cy + K));
641 
642  // Optimized code: this part will be invoked a *lot* of times:
643  {
644  cellType* mapPtr =
645  &map[xx1 + yy1 * size_x]; // Initial pointer position
646  unsigned incrAfterRow = size_x - ((xx2 - xx1) + 1);
647 
648  signed int Ax0 = 10 * (xx1 - cx);
649  signed int Ay = 10 * (yy1 - cy);
650 
651  unsigned int occupiedMinDistInt =
652  mrpt::round(maxCorrDist_sq * constDist2DiscrUnits);
653 
654  for (int yy = yy1; yy <= yy2; yy++)
655  {
656  unsigned int Ay2 =
657  square((unsigned int)(Ay)); // Square is faster
658  // with unsigned.
659  signed short Ax = Ax0;
660  cellType cell;
661 
662  for (int xx = xx1; xx <= xx2; xx++)
663  {
664  if ((cell = *mapPtr++) < thresholdCellValue)
665  {
666  unsigned int d =
667  square((unsigned int)(Ax)) + Ay2;
668  keep_min(occupiedMinDistInt, d);
669  }
670  Ax += 10;
671  }
672  // Go to (xx1,yy++)
673  mapPtr += incrAfterRow;
674  Ay += 10;
675  }
676 
677  occupiedMinDist =
678  occupiedMinDistInt * constDist2DiscrUnits_INV;
679  }
680 
681  if (likelihoodOptions.LF_useSquareDist)
682  occupiedMinDist *= occupiedMinDist;
683 
684  thisLik = zRandomTerm + zHit * exp(Q * occupiedMinDist);
685 
686  if (likelihoodOptions.enableLikelihoodCache)
687  // And save it into the table and into "thisLik":
688  precomputedLikelihood[cx + cy * size_x] = thisLik;
689  }
690  }
691 
692  // Update the likelihood:
693  if (Product_T_OrSum_F)
694  {
695  ret += log(thisLik);
696  }
697  else
698  {
699  ret += thisLik;
700  M++;
701  }
702  } // end of for each point in the scan
703 
704  if (!Product_T_OrSum_F) ret = log(ret / M);
705 
706  return ret;
707 
708  MRPT_END
709 }
710 
711 /*---------------------------------------------------------------
712  computeLikelihoodField_II
713  ---------------------------------------------------------------*/
714 double COccupancyGridMap2D::computeLikelihoodField_II(
715  const CPointsMap* pm, const CPose2D* relativePose)
716 {
717  MRPT_START
718 
719  double ret;
720  size_t N = pm->size();
721 
722  if (!N) return 1e-100; // No way to estimate this likelihood!!
723 
724  // Compute the likelihoods for each point:
725  ret = 0;
726  // if (likelihoodOptions.LF_alternateAverageMethod)
727  // ret = 0;
728  // else ret = 1;
729 
730  TPoint2D pointLocal, pointGlobal;
731 
732  float zRandomTerm = 1.0f / likelihoodOptions.LF_maxRange;
733  float Q = -0.5f / square(likelihoodOptions.LF_stdHit);
734 
735  // Aux. cell indixes variables:
736  int cx, cy;
737  size_t j;
738  int cx0, cy0;
739  int cx_min, cx_max;
740  int cy_min, cy_max;
741  int maxRangeInCells =
742  (int)ceil(likelihoodOptions.LF_maxCorrsDistance / resolution);
743  int nCells = 0;
744 
745  // -----------------------------------------------------
746  // Compute around a window of neigbors around each point
747  // -----------------------------------------------------
748  for (j = 0; j < N; j += likelihoodOptions.LF_decimation)
749  {
750  // Get the point and pass it to global coordinates:
751  // ---------------------------------------------
752  if (relativePose)
753  {
754  pm->getPoint(j, pointLocal);
755  pointGlobal = *relativePose + pointLocal;
756  }
757  else
758  {
759  pm->getPoint(j, pointGlobal);
760  }
761 
762  // Point to cell indixes:
763  // ---------------------------------------------
764  cx0 = x2idx(pointGlobal.x);
765  cy0 = y2idx(pointGlobal.y);
766 
767  // Compute the range of cells to compute:
768  // ---------------------------------------------
769  cx_min = max(cx0 - maxRangeInCells, 0);
770  cx_max = min(cx0 + maxRangeInCells, static_cast<int>(size_x));
771  cy_min = max(cy0 - maxRangeInCells, 0);
772  cy_max = min(cy0 + maxRangeInCells, static_cast<int>(size_y));
773 
774  // debugImg.rectangle(cx_min,cy_min,cx_max,cy_max,0xFF0000 );
775 
776  // Compute over the window of cells:
777  // ---------------------------------------------
778  double lik = 0;
779  for (cx = cx_min; cx <= cx_max; cx++)
780  {
781  for (cy = cy_min; cy <= cy_max; cy++)
782  {
783  float P_free = getCell(cx, cy);
784  float termDist =
785  exp(Q * (square(idx2x(cx) - pointGlobal.x) +
786  square(idx2y(cy) - pointGlobal.y)));
787 
788  lik += P_free * zRandomTerm + (1 - P_free) * termDist;
789  } // end for cy
790  } // end for cx
791 
792  // Update the likelihood:
793  if (likelihoodOptions.LF_alternateAverageMethod)
794  ret += lik;
795  else
796  ret += log(lik / ((cy_max - cy_min + 1) * (cx_max - cx_min + 1)));
797  nCells++;
798 
799  } // end of for each point in the scan
800 
801  if (likelihoodOptions.LF_alternateAverageMethod && nCells > 0)
802  ret = log(ret / nCells);
803  else
804  ret = ret / nCells;
805 
806  return ret;
807 
808  MRPT_END
809 }
810 
811 /*---------------------------------------------------------------
812  Initilization of values, don't needed to be called directly.
813  ---------------------------------------------------------------*/
814 COccupancyGridMap2D::TLikelihoodOptions::TLikelihoodOptions()
815  : OWA_weights(100, 1 / 100.0f)
816 
817 {
818 }
819 
820 /*---------------------------------------------------------------
821  loadFromConfigFile
822  ---------------------------------------------------------------*/
824  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
825 {
826  likelihoodMethod =
827  iniFile.read_enum(section, "likelihoodMethod", likelihoodMethod);
828 
829  enableLikelihoodCache = iniFile.read_bool(
830  section, "enableLikelihoodCache", enableLikelihoodCache);
831 
832  LF_stdHit = iniFile.read_float(section, "LF_stdHit", LF_stdHit);
833  LF_zHit = iniFile.read_float(section, "LF_zHit", LF_zHit);
834  LF_zRandom = iniFile.read_float(section, "LF_zRandom", LF_zRandom);
835  LF_maxRange = iniFile.read_float(section, "LF_maxRange", LF_maxRange);
836  LF_decimation = iniFile.read_int(section, "LF_decimation", LF_decimation);
837  LF_maxCorrsDistance =
838  iniFile.read_float(section, "LF_maxCorrsDistance", LF_maxCorrsDistance);
839  LF_useSquareDist =
840  iniFile.read_bool(section, "LF_useSquareDist", LF_useSquareDist);
841  LF_alternateAverageMethod = iniFile.read_bool(
842  section, "LF_alternateAverageMethod", LF_alternateAverageMethod);
843 
844  MI_exponent = iniFile.read_float(section, "MI_exponent", MI_exponent);
845  MI_skip_rays = iniFile.read_int(section, "MI_skip_rays", MI_skip_rays);
846  MI_ratio_max_distance = iniFile.read_float(
847  section, "MI_ratio_max_distance", MI_ratio_max_distance);
848 
849  rayTracing_useDistanceFilter = iniFile.read_bool(
850  section, "rayTracing_useDistanceFilter", rayTracing_useDistanceFilter);
851  rayTracing_stdHit =
852  iniFile.read_float(section, "rayTracing_stdHit", rayTracing_stdHit);
853 
854  consensus_takeEachRange = iniFile.read_int(
855  section, "consensus_takeEachRange", consensus_takeEachRange);
856  consensus_pow = iniFile.read_float(section, "consensus_pow", consensus_pow);
857 
858  iniFile.read_vector(section, "OWA_weights", OWA_weights, OWA_weights);
859 }
860 
861 /*---------------------------------------------------------------
862  dumpToTextStream
863  ---------------------------------------------------------------*/
865  std::ostream& out) const
866 {
867  out << mrpt::format(
868  "\n----------- [COccupancyGridMap2D::TLikelihoodOptions] ------------ "
869  "\n\n");
870 
871  out << mrpt::format("likelihoodMethod = ");
872  switch (likelihoodMethod)
873  {
874  case lmMeanInformation:
875  out << mrpt::format("lmMeanInformation");
876  break;
877  case lmRayTracing:
878  out << mrpt::format("lmRayTracing");
879  break;
880  case lmConsensus:
881  out << mrpt::format("lmConsensus");
882  break;
883  case lmCellsDifference:
884  out << mrpt::format("lmCellsDifference");
885  break;
887  out << mrpt::format("lmLikelihoodField_Thrun");
888  break;
890  out << mrpt::format("lmLikelihoodField_II");
891  break;
892  case lmConsensusOWA:
893  out << mrpt::format("lmConsensusOWA");
894  break;
895  default:
896  out << mrpt::format("UNKNOWN!!!");
897  break;
898  }
899  out << mrpt::format("\n");
900 
901  out << mrpt::format(
902  "enableLikelihoodCache = %c\n",
903  enableLikelihoodCache ? 'Y' : 'N');
904 
905  out << mrpt::format(
906  "LF_stdHit = %f\n", LF_stdHit);
907  out << mrpt::format(
908  "LF_zHit = %f\n", LF_zHit);
909  out << mrpt::format(
910  "LF_zRandom = %f\n", LF_zRandom);
911  out << mrpt::format(
912  "LF_maxRange = %f\n", LF_maxRange);
913  out << mrpt::format(
914  "LF_decimation = %u\n", LF_decimation);
915  out << mrpt::format(
916  "LF_maxCorrsDistance = %f\n", LF_maxCorrsDistance);
917  out << mrpt::format(
918  "LF_useSquareDist = %c\n",
919  LF_useSquareDist ? 'Y' : 'N');
920  out << mrpt::format(
921  "LF_alternateAverageMethod = %c\n",
922  LF_alternateAverageMethod ? 'Y' : 'N');
923  out << mrpt::format(
924  "MI_exponent = %f\n", MI_exponent);
925  out << mrpt::format(
926  "MI_skip_rays = %u\n", MI_skip_rays);
927  out << mrpt::format(
928  "MI_ratio_max_distance = %f\n",
929  MI_ratio_max_distance);
930  out << mrpt::format(
931  "rayTracing_useDistanceFilter = %c\n",
932  rayTracing_useDistanceFilter ? 'Y' : 'N');
933  out << mrpt::format(
934  "rayTracing_decimation = %u\n",
935  rayTracing_decimation);
936  out << mrpt::format(
937  "rayTracing_stdHit = %f\n", rayTracing_stdHit);
938  out << mrpt::format(
939  "consensus_takeEachRange = %u\n",
940  consensus_takeEachRange);
941  out << mrpt::format(
942  "consensus_pow = %.02f\n", consensus_pow);
943  out << mrpt::format("OWA_weights = [");
944  for (size_t i = 0; i < OWA_weights.size(); i++)
945  {
946  if (i < 3 || i > (OWA_weights.size() - 3))
947  out << mrpt::format("%.03f ", OWA_weights[i]);
948  else if (i == 3 && OWA_weights.size() > 6)
949  out << mrpt::format(" ... ");
950  }
951  out << mrpt::format("] (size=%u)\n", (unsigned)OWA_weights.size());
952  out << mrpt::format("\n");
953 }
954 
955 /** Returns true if this map is able to compute a sensible likelihood function
956  * for this observation (i.e. an occupancy grid map cannot with an image).
957  * \param obs The observation.
958  * \sa computeObservationLikelihood
959  */
961  const mrpt::obs::CObservation* obs) const
962 {
963  // Ignore laser scans if they are not planar or they are not
964  // at the altitude of this grid map:
966  {
967  const auto* scan = static_cast<const CObservation2DRangeScan*>(obs);
968 
969  if (!scan->isPlanarScan(insertionOptions.horizontalTolerance))
970  return false;
972  fabs(insertionOptions.mapAltitude - scan->sensorPose.z()) > 0.01)
973  return false;
974 
975  // OK, go on...
976  return true;
977  }
978  else // Is not a laser scanner...
979  {
980  return false;
981  }
982 }
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
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:203
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Definition: CPointsMap.h:251
Declares a class derived from "CObservation" that encapsules a single range measurement, and associated parameters.
#define MRPT_START
Definition: exceptions.h:282
#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...
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr)
Insert the observation information into this map.
Definition: CMetricMap.cpp:94
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 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:197
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:161
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:219
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.
#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.
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: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...
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:82
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:42
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:84
#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:286
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:255
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:271
GLuint res
Definition: glext.h:7385
GLenum GLint x
Definition: glext.h:3542
Lightweight 2D point.
size_t size() const
Returns the number of stored points in the map.
Definition: CPointsMap.h:422
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:232
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: 5887d2b31 Wed Apr 24 13:03:27 2019 +0200 at miƩ abr 24 13:10:13 CEST 2019