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



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019