Main MRPT website > C++ reference for MRPT 1.5.6
CMultiMetricMapPDF_RBPF.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 "slam-precomp.h" // Precompiled headers
11 
12 #include <mrpt/random.h>
13 #include <mrpt/math/utils.h>
14 #include <mrpt/utils/CTicTac.h>
15 #include <mrpt/utils/CFileStream.h>
16 
17 #include <mrpt/maps/CMultiMetricMapPDF.h>
18 #include <mrpt/obs/CActionRobotMovement2D.h>
19 #include <mrpt/obs/CActionRobotMovement3D.h>
20 #include <mrpt/obs/CActionCollection.h>
21 #include <mrpt/poses/CPosePDFGaussian.h>
22 #include <mrpt/poses/CPosePDFGrid.h>
23 #include <mrpt/obs/CObservationBeaconRanges.h>
24 #include <mrpt/maps/CSimplePointsMap.h>
25 #include <mrpt/maps/CLandmarksMap.h>
26 #include <mrpt/math.h>
27 
28 #include <mrpt/slam/PF_aux_structs.h>
29 
30 
31 using namespace mrpt;
32 using namespace mrpt::bayes;
33 using namespace mrpt::math;
34 using namespace mrpt::slam;
35 using namespace mrpt::obs;
36 using namespace mrpt::maps;
37 using namespace mrpt::poses;
38 using namespace mrpt::random;
39 using namespace mrpt::utils;
40 using namespace std;
41 
42 namespace mrpt
43 {
44  namespace slam
45  {
46  /** Fills out a "TPoseBin2D" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options".
47  */
48  template <>
49  void KLF_loadBinFromParticle(
50  detail::TPoseBin2D &outBin,
51  const TKLDParams &opts,
52  const mrpt::maps::CRBPFParticleData *currentParticleValue,
53  const TPose3D *newPoseToBeInserted)
54  {
55  // 2D pose approx: Use the latest pose only:
56  if (newPoseToBeInserted)
57  {
58  outBin.x = round( newPoseToBeInserted->x / opts.KLD_binSize_XY );
59  outBin.y = round( newPoseToBeInserted->y / opts.KLD_binSize_XY );
60  outBin.phi = round( newPoseToBeInserted->yaw / opts.KLD_binSize_PHI );
61  }
62  else
63  {
64  ASSERT_(currentParticleValue && !currentParticleValue->robotPath.empty())
65  const TPose3D &p = *currentParticleValue->robotPath.rbegin();
66  outBin.x = round( p.x / opts.KLD_binSize_XY );
67  outBin.y = round( p.y / opts.KLD_binSize_XY );
68  outBin.phi = round( p.yaw / opts.KLD_binSize_PHI );
69  }
70  }
71 
72  /** Fills out a "TPathBin2D" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options".
73  */
74  template <>
75  void KLF_loadBinFromParticle(
76  detail::TPathBin2D &outBin,
77  const TKLDParams &opts,
78  const mrpt::maps::CRBPFParticleData *currentParticleValue,
79  const TPose3D *newPoseToBeInserted)
80  {
81  const size_t lenBinPath = (currentParticleValue!=NULL) ? currentParticleValue->robotPath.size() : 0;
82 
83  // Set the output bin dimensionality:
84  outBin.bins.resize(lenBinPath + (newPoseToBeInserted!=NULL ? 1:0) );
85 
86  // Is a path provided??
87  if (currentParticleValue!=NULL)
88  for (size_t i=0;i<lenBinPath;++i) // Fill the bin data:
89  {
90  outBin.bins[i].x = round( currentParticleValue->robotPath[i].x / opts.KLD_binSize_XY );
91  outBin.bins[i].y = round( currentParticleValue->robotPath[i].y / opts.KLD_binSize_XY );
92  outBin.bins[i].phi = round( currentParticleValue->robotPath[i].yaw / opts.KLD_binSize_PHI );
93  }
94 
95  // Is a newPose provided??
96  if (newPoseToBeInserted!=NULL)
97  {
98  // And append the last pose: the new one:
99  outBin.bins[lenBinPath].x = round( newPoseToBeInserted->x / opts.KLD_binSize_XY );
100  outBin.bins[lenBinPath].y = round( newPoseToBeInserted->y / opts.KLD_binSize_XY );
101  outBin.bins[lenBinPath].phi = round( newPoseToBeInserted->yaw / opts.KLD_binSize_PHI );
102  }
103  }
104  }
105 }
106 
107 // Include this AFTER specializations:
108 #include <mrpt/slam/PF_implementations.h>
109 
110 /** Auxiliary for optimal sampling in RO-SLAM */
111 struct TAuxRangeMeasInfo
112 {
113  TAuxRangeMeasInfo() :
114  sensorLocationOnRobot(),
115  sensedDistance(0),
116  beaconID(INVALID_BEACON_ID),
117  nGaussiansInMap(0)
118  {}
119 
120  CPoint3D sensorLocationOnRobot;
121  float sensedDistance;
122  int64_t beaconID;
123  size_t nGaussiansInMap; // Number of Gaussian modes in the map representation
124 
125  /** Auxiliary for optimal sampling in RO-SLAM */
126  static bool cmp_Asc(const TAuxRangeMeasInfo &a, const TAuxRangeMeasInfo &b)
127  {
128  return a.nGaussiansInMap < b.nGaussiansInMap;
129  }
130 };
131 
132 
133 
134 /*----------------------------------------------------------------------------------
135  prediction_and_update_pfAuxiliaryPFOptimal
136 
137  See paper reference in "PF_SLAM_implementation_pfAuxiliaryPFOptimal"
138  ----------------------------------------------------------------------------------*/
139 void CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFOptimal(
140  const mrpt::obs::CActionCollection * actions,
141  const mrpt::obs::CSensoryFrame * sf,
142  const bayes::CParticleFilter::TParticleFilterOptions &PF_options )
143 {
144  MRPT_START
145 
146  PF_SLAM_implementation_pfAuxiliaryPFOptimal<mrpt::slam::detail::TPoseBin2D>( actions, sf, PF_options,options.KLD_params);
147 
148  MRPT_END
149 }
150 
151 /*----------------------------------------------------------------------------------
152  PF_SLAM_implementation_pfAuxiliaryPFStandard
153  ----------------------------------------------------------------------------------*/
154 void CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFStandard(
155  const mrpt::obs::CActionCollection * actions,
156  const mrpt::obs::CSensoryFrame * sf,
157  const bayes::CParticleFilter::TParticleFilterOptions &PF_options )
158 {
159  MRPT_START
160 
161  PF_SLAM_implementation_pfAuxiliaryPFStandard<mrpt::slam::detail::TPoseBin2D>( actions, sf, PF_options,options.KLD_params);
162 
163  MRPT_END
164 }
165 
166 
167 /*----------------------------------------------------------------------------------
168  prediction_and_update_pfOptimalProposal
169 
170 For grid-maps:
171 ==============
172  Approximation by Grissetti et al: Use scan matching to approximate
173  the observation model by a Gaussian:
174  See: "Improved Grid-based SLAM with Rao-Blackwellized PF by Adaptive Proposals
175  and Selective Resampling" (G. Grisetti, C. Stachniss, W. Burgard)
176 
177 For beacon maps:
178 ===============
179  (JLBC: Method under development)
180 
181  ----------------------------------------------------------------------------------*/
182 void CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal(
183  const mrpt::obs::CActionCollection * actions,
184  const mrpt::obs::CSensoryFrame * sf,
185  const bayes::CParticleFilter::TParticleFilterOptions &PF_options )
186 {
187  MRPT_START
188 
189  // ----------------------------------------------------------------------
190  // PREDICTION STAGE
191  // ----------------------------------------------------------------------
192  CVectorDouble rndSamples;
193  size_t M = m_particles.size();
194  bool updateStageAlreadyDone = false;
195  CPose3D initialPose,incrPose, finalPose;
196 
197  // ICP used if "pfOptimalProposal_mapSelection" = 0 or 1
198  CICP icp (options.icp_params); // Set our ICP params instead of default ones.
199  CICP::TReturnInfo icpInfo;
200 
201  CParticleList::iterator partIt;
202 
203  ASSERT_(sf!=NULL)
204 
205  // Find a robot movement estimation:
206  CPose3D motionModelMeanIncr; // The mean motion increment:
207  CPoseRandomSampler robotActionSampler;
208  {
209  CActionRobotMovement2DPtr robotMovement2D = actions->getBestMovementEstimation();
210 
211  // If there is no 2D action, look for a 3D action:
212  if (robotMovement2D.present())
213  {
214  robotActionSampler.setPosePDF( robotMovement2D->poseChange.get_ptr() );
215  motionModelMeanIncr = mrpt::poses::CPose3D(robotMovement2D->poseChange->getMeanVal());
216  }
217  else
218  {
219  CActionRobotMovement3DPtr robotMovement3D = actions->getActionByClass<CActionRobotMovement3D>();
220  if (robotMovement3D)
221  {
222  robotActionSampler.setPosePDF( robotMovement3D->poseChange );
223  robotMovement3D->poseChange.getMean( motionModelMeanIncr );
224  }
225  else
226  {
227  motionModelMeanIncr.setFromValues(0,0,0);
228  }
229  }
230  }
231 
232  // Average map will need to be updated after this:
233  averageMapIsUpdated = false;
234 
235  // --------------------------------------------------------------------------------------
236  // Prediction:
237  //
238  // Compute a new mean and covariance by sampling around the mean of the input "action"
239  // --------------------------------------------------------------------------------------
240  printf(" 1) Prediction...");
241  M = m_particles.size();
242 
243  // To be computed as an average from all m_particles:
244  size_t particleWithHighestW = 0;
245  for (size_t i=0;i<M;i++)
246  if (getW(i)>getW(particleWithHighestW))
247  particleWithHighestW = i;
248 
249 
250  // The paths MUST already contain the starting location for each particle:
251  ASSERT_( !m_particles[0].d->robotPath.empty() )
252 
253  // Build the local map of points for ICP:
254  CSimplePointsMap localMapPoints;
255  CLandmarksMap localMapLandmarks;
256  bool built_map_points = false;
257  bool built_map_lms = false;
258 
259  // Update particle poses:
260  size_t i;
261  for (i=0,partIt = m_particles.begin(); partIt!=m_particles.end(); partIt++,i++)
262  {
263  double extra_log_lik = 0; // Used for the optimal_PF with ICP
264 
265  // Set initial robot pose estimation for this particle:
266  const CPose3D ith_last_pose = CPose3D(*partIt->d->robotPath.rbegin()); // The last robot pose in the path
267 
268  CPose3D initialPoseEstimation = ith_last_pose + motionModelMeanIncr;
269 
270  // Use ICP with the map associated to particle?
271  if ( options.pfOptimalProposal_mapSelection==0 ||
272  options.pfOptimalProposal_mapSelection==1 ||
273  options.pfOptimalProposal_mapSelection==3 )
274  {
275  CPosePDFGaussian icpEstimation;
276 
277  // Configure the matchings that will take place in the ICP process:
278  if (partIt->d->mapTillNow.m_pointsMaps.size())
279  {
280  ASSERT_(partIt->d->mapTillNow.m_pointsMaps.size()==1);
281  //partIt->d->mapTillNow.m_pointsMaps[0]->insertionOptions.matchStaticPointsOnly = false;
282  }
283 
284  CMetricMap *map_to_align_to = NULL;
285 
286  if (options.pfOptimalProposal_mapSelection==0) // Grid map
287  {
288  ASSERT_( !partIt->d->mapTillNow.m_gridMaps.empty() );
289 
290  // Build local map of points.
291  if (!built_map_points)
292  {
293  built_map_points=true;
294 
295  localMapPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f; //3.0f * m_particles[0].d->mapTillNow.m_gridMaps[0]->getResolution();;
296  localMapPoints.insertionOptions.isPlanarMap = true;
297  sf->insertObservationsInto( &localMapPoints );
298  }
299 
300  map_to_align_to = partIt->d->mapTillNow.m_gridMaps[0].pointer();
301  }
302  else
303  if (options.pfOptimalProposal_mapSelection==3) // Map of points
304  {
305  ASSERT_( !partIt->d->mapTillNow.m_pointsMaps.empty() );
306 
307  // Build local map of points.
308  if (!built_map_points)
309  {
310  built_map_points=true;
311 
312  localMapPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f; //3.0f * m_particles[0].d->mapTillNow.m_gridMaps[0]->getResolution();;
313  localMapPoints.insertionOptions.isPlanarMap = true;
314  sf->insertObservationsInto( &localMapPoints );
315  }
316 
317  map_to_align_to = partIt->d->mapTillNow.m_pointsMaps[0].pointer();
318  }
319  else
320  {
321  ASSERT_( partIt->d->mapTillNow.m_landmarksMap.present() );
322 
323  // Build local map of LMs.
324  if (!built_map_lms)
325  {
326  built_map_lms=true;
327  sf->insertObservationsInto( &localMapLandmarks );
328  }
329 
330  map_to_align_to = partIt->d->mapTillNow.m_landmarksMap.pointer();
331  }
332 
333  ASSERT_(map_to_align_to!=NULL);
334 
335  // Use ICP to align to each particle's map:
336  {
337  CPosePDFPtr alignEst =
338  icp.Align(
339  map_to_align_to,
340  &localMapPoints,
341  CPose2D(initialPoseEstimation),
342  NULL,
343  &icpInfo);
344  icpEstimation.copyFrom( *alignEst );
345  }
346 
347 
348  if (i==particleWithHighestW)
349  {
350  newInfoIndex = 1 - icpInfo.goodness; //newStaticPointsRatio; //* icpInfo.goodness;
351  }
352 
353  // Set the gaussian pose:
354  CPose3DPDFGaussian finalEstimatedPoseGauss( icpEstimation );
355 
356  //printf("[rbpf-slam] gridICP[%u]: %.02f%%\n", i, 100*icpInfo.goodness);
357  if (icpInfo.goodness<options.ICPGlobalAlign_MinQuality && SFs.size())
358  {
359  printf("[rbpf-slam] Warning: gridICP[%u]: %.02f%% -> Using odometry instead!\n", (unsigned int)i, 100*icpInfo.goodness);
360  icpEstimation.mean = CPose2D(initialPoseEstimation);
361  }
362 
363  // As a way to take into account the odometry / "prior", use
364  // a correcting factor in the likelihood from the mismatch prior<->icp_estimate:
365 // const double prior_dist_lin = initialPoseEstimation.distanceTo(icpEstimation.mean);
366 // const double prior_dist_ang = std::abs( mrpt::math::wrapToPi( initialPoseEstimation.yaw()-icpEstimation.mean.phi() ) );
367 //// if (prior_dist_lin>0.10 || prior_dist_ang>DEG2RAD(3))
368 //// printf(" >>>>>>>>>> %f %f\n",prior_dist_lin,RAD2DEG(prior_dist_ang));
369 // extra_log_lik = -(prior_dist_lin/0.20) - (prior_dist_ang/DEG2RAD(20));
370 
371 // printf("gICP: %.02f%%, Iters=%u\n",icpInfo.goodness,icpInfo.nIterations);
372 
373 #if 0 // Use hacked ICP covariance:
374  CPose3D Ap = finalEstimatedPoseGauss.mean - ith_last_pose;
375  const double Ap_dist = Ap.norm();
376 
377  finalEstimatedPoseGauss.cov.zeros();
378  finalEstimatedPoseGauss.cov(0,0) = square( fabs(Ap_dist)*0.01 );
379  finalEstimatedPoseGauss.cov(1,1) = square( fabs(Ap_dist)*0.01 );
380  finalEstimatedPoseGauss.cov(2,2) = square( fabs(Ap.yaw())*0.02 );
381 #else
382  // Use real ICP covariance (with a minimum level):
383  keep_max( finalEstimatedPoseGauss.cov(0,0), square(0.002));
384  keep_max( finalEstimatedPoseGauss.cov(1,1), square(0.002));
385  keep_max( finalEstimatedPoseGauss.cov(2,2), square(DEG2RAD(0.1)));
386 
387 #endif
388 
389  // Generate gaussian-distributed 2D-pose increments according to "finalEstimatedPoseGauss":
390  // -------------------------------------------------------------------------------------------
391  finalPose = finalEstimatedPoseGauss.mean; // Add to the new robot pose:
392  randomGenerator.drawGaussianMultivariate(rndSamples, finalEstimatedPoseGauss.cov );
393  // Add noise:
394  finalPose.setFromValues(
395  finalPose.x() + rndSamples[0],
396  finalPose.y() + rndSamples[1],
397  finalPose.z(),
398  finalPose.yaw() + rndSamples[2],
399  finalPose.pitch(),
400  finalPose.roll() );
401  }
402  else
403  if ( options.pfOptimalProposal_mapSelection==2)
404  {
405  // --------------------------------------------------------
406  // Perform optimal sampling with the beacon map:
407  // Described in paper...
408  // --------------------------------------------------------
409  /** \todo Add paper ref!
410  */
411  ASSERT_( partIt->d->mapTillNow.m_beaconMap.present() );
412  CBeaconMapPtr beacMap = partIt->d->mapTillNow.m_beaconMap;
413 
414  updateStageAlreadyDone = true; // We'll also update the weight of the particle here
415 
416  // =====================================================================
417  // SUMMARY:
418  // For each beacon measurement in the SF, stored in "lstObservedRanges",
419  // compute the SOG of the observation model, and multiply all of them
420  // (fuse) in "fusedObsModels". The result will hopefully be a very small
421  // PDF where to draw a sample from for the new robot pose.
422  // =====================================================================
423  bool methodSOGorGrid = false; // TRUE=SOG
424  CPoint3D newDrawnPosition;
425  float firstEstimateRobotHeading=std::numeric_limits<float>::max();
426 
427  // The parameters to discard too far gaussians:
428  CPoint3D centerPositionPrior( ith_last_pose );
429  float centerPositionPriorRadius=2.0f;
430 
431  if ( !robotActionSampler.isPrepared() )
432  {
433  firstEstimateRobotHeading = ith_last_pose.yaw();
434  // If the map is empty: There is no solution!:
435  // THROW_EXCEPTION("There is no odometry & the initial beacon map is empty: RO-SLAM has no solution -> ABORTED!!!");
436 
437  if (!beacMap->size())
438  {
439  // First iteration only...
440  cerr << "[RO-SLAM] Optimal filtering without map & odometry...this message should appear only the first iteration!!" << endl;
441  }
442  else
443  {
444  // To make RO-SLAM to have a solution, arbitrarily fix one of the beacons so
445  // unbiguity dissapears.
446  if ( beacMap->get(0).m_typePDF==CBeacon::pdfSOG)
447  {
448  cerr << "[RO-SLAM] Optimal filtering without map & odometry->FIXING ONE BEACON!" << endl;
449  ASSERT_(beacMap->get(0).m_locationSOG.size()>0)
450 
451  CPoint3D fixedBeacon( beacMap->get(0).m_locationSOG[0].val.mean );
452 
453  // Pass to gaussian without uncertainty:
454  beacMap->get(0).m_typePDF=CBeacon::pdfGauss;
455  beacMap->get(0).m_locationSOG.clear();
456  beacMap->get(0).m_locationGauss.mean = fixedBeacon;
457  beacMap->get(0).m_locationGauss.cov.unit(3, 1e-6);
458  }
459  }
460  } // end if there is no odometry
461 
462 
463  // 1. Make the list of beacon IDs-ranges:
464  // -------------------------------------------
465  deque<TAuxRangeMeasInfo> lstObservedRanges;
466 
467  for (CSensoryFrame::const_iterator itObs = sf->begin();itObs!=sf->end();++itObs)
468  {
469  if ( IS_CLASS( (*itObs),CObservationBeaconRanges) )
470  {
471  const CObservationBeaconRanges *obs = static_cast<const CObservationBeaconRanges*> (itObs->pointer());
472  deque<CObservationBeaconRanges::TMeasurement>::const_iterator itRanges;
473  for (itRanges=obs->sensedData.begin();itRanges!=obs->sensedData.end();itRanges++)
474  {
475  ASSERT_( itRanges->beaconID != INVALID_BEACON_ID )
476  // only add those in the map:
477  for (CBeaconMap::iterator itBeacs=beacMap->begin();itBeacs!=beacMap->end();++itBeacs)
478  {
479  if ( (itBeacs)->m_ID == itRanges->beaconID)
480  {
481  TAuxRangeMeasInfo newMeas;
482  newMeas.beaconID = itRanges->beaconID;
483  newMeas.sensedDistance = itRanges->sensedDistance;
484  newMeas.sensorLocationOnRobot = itRanges->sensorLocationOnRobot;
485 
486  ASSERT_( (itBeacs)->m_typePDF==CBeacon::pdfGauss || (itBeacs)->m_typePDF==CBeacon::pdfSOG )
487  newMeas.nGaussiansInMap = (itBeacs)->m_typePDF==CBeacon::pdfSOG ? (itBeacs)->m_locationSOG.size() : 1 /*pdfGauss*/;
488 
489  lstObservedRanges.push_back( newMeas );
490  break; // Next observation
491  }
492  }
493  }
494  }
495  }
496 
497  //ASSERT_( lstObservedRanges.size()>=2 );
498 
499  // Sort ascending ranges: Smallest ranges first -> the problem is easiest!
500  sort( lstObservedRanges.begin(),lstObservedRanges.end(), &TAuxRangeMeasInfo::cmp_Asc);
501 
502 
503  if ( methodSOGorGrid )
504  {
505  CPointPDFSOG fusedObsModels; //<- p(z_t|x_t) for all the range measurements (fused)
506  fusedObsModels.clear();
507 
508  // 0. OPTIONAL: Create a "prior" as a first mode in "fusedObsModels"
509  // using odometry. If there is no odometry, we *absolutely* need
510  // at least one well-localized beacon at the beginning, or the symmetry cannot be broken!
511  // -----------------------------------------------------------------------------------------------
512  if ( robotActionSampler.isPrepared() )
513  {
514  CPointPDFSOG::TGaussianMode newMode;
515  newMode.log_w = 0;
516 
517  CPose3D auxPose= ith_last_pose + motionModelMeanIncr; // CPose3D(robotMovement->poseChange->getEstimatedPose()));
518  firstEstimateRobotHeading = auxPose.yaw();
519 
520  newMode.val.mean = CPoint3D(auxPose);
521 
522  // Uncertainty in z is null:
523  //CMatrix poseCOV = robotMovement->poseChange->getEstimatedCovariance();
524  CMatrixD poseCOV;
525  robotActionSampler.getOriginalPDFCov2D( poseCOV );
526 
527  poseCOV.setSize(2,2);
528  poseCOV.setSize(3,3);
529  newMode.val.cov =poseCOV;
530  fusedObsModels.push_back(newMode); // Add it:
531  }
532 
533 
534  // 2. Generate the optimal proposal by fusing obs models
535  // -------------------------------------------------------------
536  for (CBeaconMap::iterator itBeacs=beacMap->begin();itBeacs!=beacMap->end();++itBeacs)
537  {
538  // for each observed beacon (by its ID), generate observation model:
539  for (deque<TAuxRangeMeasInfo>::iterator itObs=lstObservedRanges.begin();itObs!=lstObservedRanges.end();++itObs)
540  {
541  if ((itBeacs)->m_ID==itObs->beaconID)
542  {
543  // Match:
544  float sensedRange = itObs->sensedDistance;
545 
546  CPointPDFSOG newObsModel;
547  (itBeacs)->generateObservationModelDistribution(
548  sensedRange,
549  newObsModel,
550  beacMap.pointer(), // The beacon map, for options
551  itObs->sensorLocationOnRobot,// Sensor location on robot
552  centerPositionPrior,
553  centerPositionPriorRadius );
554 
555  if (! fusedObsModels.size() )
556  {
557  // This is the first one: Just copy the obs. model here:
558  fusedObsModels = newObsModel;
559  }
560  else
561  {
562  // Fuse with existing:
563  CPointPDFSOG tempFused(0);
564  tempFused.bayesianFusion(
565  fusedObsModels,
566  newObsModel,
567  3 // minMahalanobisDistToDrop
568  );
569  fusedObsModels = tempFused;
570  }
571 
572  // Remove modes with negligible weights:
573  // -----------------------------------------------------------
574 
575  {
576  cout << "#modes bef: " << fusedObsModels.size() << " ESS: " << fusedObsModels.ESS() << endl;
577  double max_w=-1e100;
578  //int idx;
579 
580  CPointPDFSOG::iterator it;
581 
582  for (it=fusedObsModels.begin();it!=fusedObsModels.end();it++)
583  max_w = max(max_w,(it)->log_w); // keep the maximum mode weight
584 
585  for (it=fusedObsModels.begin();it!=fusedObsModels.end(); )
586  {
587  if (max_w - (it)->log_w > 20 ) // Remove the mode:
588  it = fusedObsModels.erase( it );
589  else it++;
590  }
591 
592  cout << "#modes after: " << fusedObsModels.size() << endl;
593  }
594 
595  // Shall we simplify the PDF?
596  // -----------------------------------------------------------
597  CMatrixDouble currentCov;
598  fusedObsModels.getCovariance(currentCov);
599  ASSERT_(currentCov(0,0)>0 && currentCov(1,1)>0)
600  if ( sqrt(currentCov(0,0))< 0.10f &&
601  sqrt(currentCov(1,1))< 0.10f &&
602  sqrt(currentCov(2,2))< 0.10f )
603  {
604  // Approximate by a single gaussian!
605  CPoint3D currentMean;
606  fusedObsModels.getMean(currentMean);
607  fusedObsModels[0].log_w = 0;
608  fusedObsModels[0].val.mean = currentMean;
609  fusedObsModels[0].val.cov = currentCov;
610 
611  fusedObsModels.resize(1);
612  }
613 
614 
615  {/*
616  CMatrixD evalGrid;
617  fusedObsModels.evaluatePDFInArea(-3,3,-3,3,0.1,0,evalGrid, true);
618  evalGrid *= 1.0/evalGrid.maxCoeff();
619  CImage imgF(evalGrid, true);
620  static int autoCount=0;
621  imgF.saveToFile(format("debug_%04i.png",autoCount++));*/
622  }
623  }
624  } // end for itObs (in lstObservedRanges)
625 
626  } // end for itBeacs
627 
628 
629  /** /
630  COpenGLScene scene;
631  opengl::CSetOfObjects *obj = new opengl::CSetOfObjects();
632  fusedObsModels.getAs3DObject( *obj );
633  scene.insert( obj );
634  CFileStream("debug.3Dscene",fomWrite) << scene;
635  cout << "fusedObsModels # of modes: " << fusedObsModels.m_modes.size() << endl;
636  printf("ESS: %f\n",fusedObsModels.ESS() );
637  cout << fusedObsModels.getEstimatedCovariance() << endl;
638  mrpt::system::pause(); / **/
639 
640  if (beacMap->size())
641  fusedObsModels.drawSingleSample( newDrawnPosition );
642  }
643  else
644  {
645  // =============================
646  // GRID METHOD
647  // =============================
648 
649  float grid_min_x = ith_last_pose.x() - 0.5f;
650  float grid_max_x = ith_last_pose.x() + 0.5f;
651  float grid_min_y = ith_last_pose.y() - 0.5f;
652  float grid_max_y = ith_last_pose.y() + 0.5f;
653  float grid_resXY = 0.02f;
654 
655  bool repeatGridCalculation=false;
656 
657  do
658  {
659  CPosePDFGrid *pdfGrid = new CPosePDFGrid(
660  grid_min_x,grid_max_x,
661  grid_min_y,grid_max_y,
662  grid_resXY, DEG2RAD(180), 0, 0 );
663 
664  pdfGrid->uniformDistribution();
665 
666  // Fuse all the observation models in the grid:
667  // -----------------------------------------------------
668  for (CBeaconMap::iterator itBeacs=beacMap->begin();itBeacs!=beacMap->end();++itBeacs)
669  {
670  // for each observed beacon (by its ID), generate observation model:
671  for (deque<TAuxRangeMeasInfo>::iterator itObs=lstObservedRanges.begin();itObs!=lstObservedRanges.end();++itObs)
672  {
673  if ((itBeacs)->m_ID==itObs->beaconID)
674  {
675  // Match:
676  float sensedRange = itObs->sensedDistance;
677 
678 /** /
679  CPointPDFSOG newObsModel;
680  (itBeacs)->generateObservationModelDistribution(
681  sensedRange,
682  newObsModel,
683  beacMap, // The beacon map, for options
684  itObs->sensorLocationOnRobot,// Sensor location on robot
685  centerPositionPrior,
686  centerPositionPriorRadius );
687 / **/
688  for (size_t idxX=0;idxX<pdfGrid->getSizeX();idxX++)
689  {
690  float grid_x = pdfGrid->idx2x(idxX);
691  for (size_t idxY=0;idxY<pdfGrid->getSizeY();idxY++)
692  {
693  double grid_y = pdfGrid->idx2y(idxY);
694 
695  // Evaluate obs model:
696  double *cell = pdfGrid->getByIndex(idxX,idxY,0);
697 
698  //
699  double lik = 1; //newObsModel.evaluatePDF(CPoint3D(grid_x,grid_y,0),true);
700  switch ((itBeacs)->m_typePDF)
701  {
702  case CBeacon::pdfSOG:
703  {
704  CPointPDFSOG *sog = & (itBeacs)->m_locationSOG;
705  double sensorSTD2 = square(beacMap->likelihoodOptions.rangeStd);
706 
707  CPointPDFSOG::iterator it;
708  for (it = sog->begin();it!=sog->end();it++)
709  {
710  lik *= exp( -0.5*square( sensedRange -
711  (it)->val.mean.distance2DTo( grid_x+itObs->sensorLocationOnRobot.x(), grid_y+itObs->sensorLocationOnRobot.y()))/sensorSTD2 );
712  }
713  }
714  break;
715  default: break; //THROW_EXCEPTION("NO")
716 
717  }
718 
719  (*cell) *= lik;
720 
721  } // for idxY
722  } // for idxX
723  pdfGrid->normalize();
724  } // end if match
725  } // end for itObs
726  } // end for beacons in map
727 
728  // Draw the single pose from the grid:
729  if (beacMap->size())
730  {
731  // Take the most likely cell:
732  float maxW = -1e10f;
733  float maxX=0,maxY=0;
734  for (size_t idxX=0;idxX<pdfGrid->getSizeX();idxX++)
735  {
736  //float grid_x = pdfGrid->idx2x(idxX);
737  for (size_t idxY=0;idxY<pdfGrid->getSizeY();idxY++)
738  {
739  //float grid_y = pdfGrid->idx2y(idxY);
740 
741  // Evaluate obs model:
742  float c = *pdfGrid->getByIndex(idxX,idxY,0);
743  if (c>maxW)
744  {
745  maxW=c;
746  maxX=pdfGrid->idx2x(idxX);
747  maxY=pdfGrid->idx2y(idxY);
748  }
749  }
750  }
751  newDrawnPosition.x( maxX );
752  newDrawnPosition.y( maxY );
753 
754  #if 0
755  {
756  //cout << "Grid: " << pdfGaussApprox << endl;
757  //pdfGrid->saveToTextFile("debug.txt");
758  CMatrixDouble outMat;
759  pdfGrid->getAsMatrix(0, outMat );
760  outMat *= 1.0f/outMat.maxCoeff();
761  CImage imgF(outMat, true);
762  static int autocount=0;
763  imgF.saveToFile(format("debug_grid_%f_%05i.png",grid_resXY,autocount++));
764  printf("grid res: %f MAX: %f,%f\n",grid_resXY,maxX,maxY);
765  //mrpt::system::pause();
766  }
767 #endif
768 
769  if (grid_resXY>0.01f)
770  {
771  grid_min_x = maxX - 0.03f;
772  grid_max_x = maxX + 0.03f;
773  grid_min_y = maxY - 0.03f;
774  grid_max_y = maxY + 0.03f;
775  grid_resXY=0.002f;
776  repeatGridCalculation = true;
777  }
778  else
779  repeatGridCalculation = false;
780 
781 
782  /*
783  // Approximate by a Gaussian:
784  CPosePDFGaussian pdfGaussApprox(
785  pdfGrid->getEstimatedPose(),
786  pdfGrid->getEstimatedCovariance() );
787  CPose2D newDrawnPose;
788  //pdfGrid->drawSingleSample( newDrawnPose );
789  pdfGaussApprox.drawSingleSample( newDrawnPose );
790  newDrawnPosition = newDrawnPose;
791  */
792  }
793  delete pdfGrid; pdfGrid = NULL;
794 
795  } while (repeatGridCalculation);
796 
797  // newDrawnPosition has the pose:
798  }
799 
800  // 3. Get the new "finalPose" of this particle as a random sample from the
801  // optimal proposal:
802  // ---------------------------------------------------------------------------
803  if (!beacMap->size())
804  {
805  // If we are in the first iteration (no map yet!) just stay still:
806  finalPose = ith_last_pose;
807  }
808  else
809  {
810  cout << "Drawn: " << newDrawnPosition << endl;
811  //cout << "Final cov was:\n" << fusedObsModels.getEstimatedCovariance() << endl << endl;
812 
813  ASSERT_(firstEstimateRobotHeading!=std::numeric_limits<float>::max()) // Make sure it was initialized
814 
815  finalPose.setFromValues(
816  newDrawnPosition.x(),
817  newDrawnPosition.y(),
818  newDrawnPosition.z(),
819  firstEstimateRobotHeading, 0,0);
820  }
821  /** \todo If there are 2+ sensors on the robot, compute phi?
822  */
823 
824  // 4. Update the weight:
825  // In optimal sampling this is the expectation of the
826  // observation likelihood: This is the observation likelihood averaged
827  // over the whole optimal proposal.
828  // ---------------------------------------------------------------------------
829  double weightUpdate=0;
830  partIt->log_w += PF_options.powFactor * weightUpdate;
831 
832  }
833  else
834  {
835  // By default:
836  // Generate gaussian-distributed 2D-pose increments according to mean-cov:
837  if ( !robotActionSampler.isPrepared() )
838  THROW_EXCEPTION("Action list does not contain any CActionRobotMovement2D or CActionRobotMovement3D object!");
839 
840  robotActionSampler.drawSample( incrPose );
841 
842  finalPose = ith_last_pose + incrPose;
843  }
844 
845  // Insert as the new pose in the path:
846  partIt->d->robotPath.push_back( finalPose );
847 
848  // ----------------------------------------------------------------------
849  // UPDATE STAGE
850  // ----------------------------------------------------------------------
851  if (!updateStageAlreadyDone)
852  {
853  partIt->log_w +=
854  PF_options.powFactor *
855  (PF_SLAM_computeObservationLikelihoodForParticle(PF_options,i,*sf,finalPose)
856  + extra_log_lik);
857  } // if update not already done...
858 
859  } // end of for each particle "i" & "partIt"
860 
861  printf("Ok\n");
862 
863  MRPT_END
864 }
865 
866 /*---------------------------------------------------------------
867  prediction_and_update_pfStandardProposal
868  ---------------------------------------------------------------*/
869 void CMultiMetricMapPDF::prediction_and_update_pfStandardProposal(
870  const mrpt::obs::CActionCollection * actions,
871  const mrpt::obs::CSensoryFrame * sf,
872  const bayes::CParticleFilter::TParticleFilterOptions &PF_options )
873 {
874  MRPT_START
875 
876  PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(actions, sf, PF_options,options.KLD_params);
877 
878  // Average map will need to be updated after this:
879  averageMapIsUpdated = false;
880 
881  MRPT_END
882 }
883 
884 
885 // Specialization for my kind of particles:
886 void CMultiMetricMapPDF::PF_SLAM_implementation_custom_update_particle_with_new_pose(
887  CRBPFParticleData *particleData,
888  const TPose3D &newPose) const
889 {
890  particleData->robotPath.push_back( newPose );
891 }
892 
893 // Specialization for RBPF maps:
894 bool CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations(
895  const CMultiMetricMapPDF::CParticleList &particles,
896  const CSensoryFrame *sf) const
897 {
898  if (sf==NULL) return false;
899  ASSERT_(!particles.empty())
900  return particles.begin()->d.get()->mapTillNow.canComputeObservationsLikelihood( *sf );
901 }
902 
903 /** Do not move the particles until the map is populated. */
904 bool CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement() const
905 {
906  return 0==getNumberOfObservationsInSimplemap();
907 }
908 
909 
910 
911 /*---------------------------------------------------------------
912  Evaluate the observation likelihood for one
913  particle at a given location
914  ---------------------------------------------------------------*/
915 double CMultiMetricMapPDF::PF_SLAM_computeObservationLikelihoodForParticle(
916  const CParticleFilter::TParticleFilterOptions &PF_options,
917  const size_t particleIndexForMap,
918  const CSensoryFrame &observation,
919  const CPose3D &x ) const
920 {
921  MRPT_UNUSED_PARAM(PF_options);
922  CMultiMetricMap *map = const_cast<CMultiMetricMap *>(&m_particles[particleIndexForMap].d->mapTillNow);
923  double ret = 0;
924  for (CSensoryFrame::const_iterator it=observation.begin();it!=observation.end();++it)
925  ret += map->computeObservationLikelihood( (CObservation*)it->pointer(), x );
926  return ret;
927 }
928 
mrpt::random
A namespace of pseudo-random numbers genrators of diferent distributions.
Definition: RandomGenerators.h:22
mrpt::poses::CPointPDFSOG::drawSingleSample
void drawSingleSample(CPoint3D &outSample) const MRPT_OVERRIDE
Draw a sample from the pdf.
Definition: CPointPDFSOG.cpp:250
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
mrpt::utils
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:46
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:73
CPosePDFGrid.h
PF_aux_structs.h
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:30
mrpt::slam::CMetricMapsAlignmentAlgorithm::Align
mrpt::poses::CPosePDFPtr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, float *runningTime=NULL, void *info=NULL)
The method for aligning a pair of metric maps, aligning only 2D + orientation.
Definition: CMetricMapsAlignmentAlgorithm.cpp:26
TAuxRangeMeasInfo::cmp_Asc
static bool cmp_Asc(const TAuxRangeMeasInfo &a, const TAuxRangeMeasInfo &b)
Auxiliary for optimal sampling in RO-SLAM.
Definition: CMultiMetricMapPDF_RBPF.cpp:126
mrpt::utils::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
CFileStream.h
mrpt::poses::CPointPDFGaussian::mean
CPoint3D mean
The mean value.
Definition: CPointPDFGaussian.h:44
mrpt::slam::CICP
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:52
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CParticleFilter.h:22
mrpt::poses::CPointPDFSOG::resize
void resize(const size_t N)
Resize the number of SOG modes.
Definition: CPointPDFSOG.cpp:52
mrpt::poses::CPointPDFSOG
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:35
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: maps/CColouredPointsMap.h:154
mrpt::poses::CPointPDFSOG::TGaussianMode
The struct for each mode:
Definition: CPointPDFSOG.h:43
mrpt::random::randomGenerator
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:23
mrpt::maps::CMetricMap::computeObservationLikelihood
double computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom)
Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.
Definition: CMetricMap.cpp:196
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::poses::CPosePDFGrid::uniformDistribution
void uniformDistribution()
Assigns the same value to all the cells in the grid, so the sum 1.
Definition: CPosePDFGrid.cpp:274
PF_implementations.h
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
mrpt::math::dynamic_vector
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
mrpt::poses::CPointPDFSOG::clear
void clear()
Clear all the gaussian modes.
Definition: CPointPDFSOG.cpp:44
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
mrpt::obs::CObservationBeaconRanges
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Definition: obs/CObservationBeaconRanges.h:28
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:22
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
Definition: maps/CSimplePointsMap.h:33
mrpt::utils::CProbabilityDensityFunction::getCovariance
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
Definition: CProbabilityDensityFunction.h:68
std
STL namespace.
mrpt::maps::CBeaconMap::iterator
std::deque< CBeacon >::iterator iterator
Definition: maps/CBeaconMap.h:51
const_iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::math::TPose3D::yaw
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: lightweight_geom_data.h:409
mrpt::poses::CPoseRandomSampler::isPrepared
bool isPrepared() const
Return true if samples can be generated, which only requires a previous call to setPosePDF.
Definition: CPoseRandomSampler.cpp:379
mrpt::slam::KLF_loadBinFromParticle
void KLF_loadBinFromParticle(detail::TPathBin2D &outBin, const TKLDParams &opts, const mrpt::maps::CRBPFParticleData *currentParticleValue, const TPose3D *newPoseToBeInserted)
Fills out a "TPathBin2D" variable, given a path hypotesis and (if not set to NULL) a new pose appende...
Definition: CMultiMetricMapPDF_RBPF.cpp:75
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: obs/CActionCollection.h:31
mrpt::poses::CPose2DGridTemplate::idx2y
double idx2y(size_t y) const
Returns coordinates from "indexes":
Definition: CPose2DGridTemplate.h:84
mrpt::obs::CSensoryFrame::const_iterator
std::deque< CObservationPtr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
Definition: obs/CSensoryFrame.h:197
mrpt::math::square
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
mrpt::poses::CPose2DGridTemplate::getAsMatrix
void getAsMatrix(const double &phi, MATRIXLIKE &outMat)
Returns the whole grid as a matrix, for a given constant "phi" and where each row contains values for...
Definition: CPose2DGridTemplate.h:200
mrpt::poses::CPose2DGridTemplate::getByIndex
const T * getByIndex(size_t x, size_t y, size_t phi) const
Reads the contents of a cell.
Definition: CPose2DGridTemplate.h:183
mrpt::obs::CSensoryFrame::begin
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
Definition: obs/CSensoryFrame.h:210
mrpt::obs::CActionRobotMovement3D
Represents a probabilistic 3D (6D) movement.
Definition: obs/CActionRobotMovement3D.h:27
mrpt::slam::CICP::TReturnInfo::goodness
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:137
mrpt::math
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
mrpt::slam::detail::TPathBin2D
Auxiliary structure.
Definition: PF_aux_structs.h:51
mrpt::maps::CLandmarksMap
A class for storing a map of 3D probabilistic landmarks.
Definition: maps/CLandmarksMap.h:59
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Definition: CPosePDFGaussian.h:32
MRPT_END
#define MRPT_END
Definition: maps/CColouredPointsMap.h:370
mrpt::poses::CPoseRandomSampler::setPosePDF
void setPosePDF(const CPosePDF *pdf)
This method must be called to select the PDF from which to draw samples.
Definition: CPoseRandomSampler.cpp:129
MRPT_UNUSED_PARAM
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: maps/CColouredPointsMap.h:307
mrpt::poses::CPoseOrPoint::norm
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
int64_t
__int64 int64_t
Definition: rptypes.h:51
mrpt::poses::CPointPDFSOG::push_back
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
Definition: CPointPDFSOG.h:99
mrpt::poses::CPoseRandomSampler::getOriginalPDFCov2D
void getOriginalPDFCov2D(mrpt::math::CMatrixDouble33 &cov3x3) const
Retrieves the 3x3 covariance of the original PDF in .
Definition: CPoseRandomSampler.cpp:387
c
const GLubyte * c
Definition: glext.h:5590
mrpt::poses::CPointPDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPointPDFGaussian.h:45
mrpt::obs::CObservationBeaconRanges::sensedData
std::deque< TMeasurement > sensedData
The list of observed ranges.
Definition: obs/CObservationBeaconRanges.h:50
mrpt::poses::CPointPDFSOG::getMean
void getMean(CPoint3D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
Definition: CPointPDFSOG.cpp:62
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:18
val
int val
Definition: mrpt_jpeglib.h:953
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: obs/CSensoryFrame.h:52
mrpt::format
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
b
GLubyte GLubyte b
Definition: glext.h:5575
mrpt::slam::TKLDParams::KLD_binSize_XY
double KLD_binSize_XY
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox&#39;s papers), which is used only i...
Definition: TKLDParams.h:32
DEG2RAD
#define DEG2RAD
Definition: maps/CColouredPointsMap.h:99
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:32
mrpt::utils::keep_max
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value...
Definition: bits.h:176
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
mrpt::maps::CRBPFParticleData
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
Definition: maps/CMultiMetricMapPDF.h:37
mrpt::poses::CPointPDFSOG::erase
iterator erase(iterator i)
Definition: CPointPDFSOG.h:108
CTicTac.h
mrpt::slam::detail::TPoseBin2D
Auxiliary structure used in KLD-sampling in particle filters.
Definition: PF_aux_structs.h:30
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:77
INVALID_BEACON_ID
#define INVALID_BEACON_ID
Used for CObservationBeaconRange, CBeacon, etc.
Definition: obs/CObservation.h:24
mrpt::poses::CPointPDFSOG::bayesianFusion
void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
Definition: CPointPDFSOG.cpp:292
MRPT_START
#define MRPT_START
Definition: maps/CColouredPointsMap.h:366
CPosePDFGaussian.h
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: maps/CMetricMap.h:52
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
mrpt::obs::CObservation
Declares a class that represents any robot&#39;s observation.
Definition: obs/CObservation.h:40
mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
Definition: CParticleFilter.h:94
mrpt::poses::CPointPDFSOG::ESS
double ESS() const
Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights o...
Definition: CPointPDFSOG.cpp:450
IS_CLASS
#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:103
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:80
mrpt::poses::CPointPDFSOG::size
size_t size() const
Return the number of Gaussian modes.
Definition: CPointPDFSOG.h:111
mrpt::obs::CActionCollection::getBestMovementEstimation
CActionRobotMovement2DPtr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
Definition: CActionCollection.cpp:133
mrpt::maps::CRBPFParticleData::robotPath
std::deque< mrpt::math::TPose3D > robotPath
Definition: maps/CMultiMetricMapPDF.h:49
mrpt::poses::CPose3D::setFromValues
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:254
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:128
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:406
ASSERT_
#define ASSERT_(f)
Definition: maps/CColouredPointsMap.h:278
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
Definition: CPose3DPDFGaussian.h:37
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
mrpt::utils::CImage::saveToFile
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV)...
Definition: CImage.cpp:299
mrpt::slam::detail::TPathBin2D::bins
std::vector< TPoseBin2D > bins
Definition: PF_aux_structs.h:53
mrpt::poses::CPose2DGridTemplate::idx2x
double idx2x(size_t x) const
Returns coordinates from "indexes":
Definition: CPose2DGridTemplate.h:76
mrpt::obs::CActionCollection::getActionByClass
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i&#39;th action of a given class, or a NULL smart pointer if there is no action of that cla...
Definition: obs/CActionCollection.h:127
TAuxRangeMeasInfo
Auxiliary for optimal sampling in RO-SLAM.
Definition: CMultiMetricMapPDF_RBPF.cpp:111
mrpt::poses::CPoseRandomSampler::drawSample
CPose2D & drawSample(CPose2D &p) const
Generate a new sample from the selected PDF.
Definition: CPoseRandomSampler.cpp:225
mrpt::poses::CPointPDFSOG::iterator
std::deque< TGaussianMode >::iterator iterator
Definition: CPointPDFSOG.h:58
mrpt::random::CRandomGenerator::drawGaussianMultivariate
void drawGaussianMultivariate(std::vector< T > &out_result, const mrpt::math::CMatrixTemplateNumeric< T > &cov, const std::vector< T > *mean=NULL)
Generate multidimensional random samples according to a given covariance matrix.
Definition: RandomGenerator.cpp:191
x
GLenum GLint x
Definition: glext.h:3516
mrpt::obs::CSensoryFrame::end
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
Definition: obs/CSensoryFrame.h:223
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: maps/CMultiMetricMap.h:123
mrpt::poses::CPoseRandomSampler
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
Definition: CPoseRandomSampler.h:35
mrpt::poses::CPosePDFGrid
Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x...
Definition: CPosePDFGrid.h:30
a
GLubyte GLubyte GLubyte a
Definition: glext.h:5575
p
GLfloat GLfloat p
Definition: glext.h:5587
mrpt::poses::CPosePDFGrid::normalize
void normalize()
Normalizes the PDF, such as all cells sum the unity.
Definition: CPosePDFGrid.cpp:257
mrpt::obs::CSensoryFrame::insertObservationsInto
bool insertObservationsInto(mrpt::maps::CMetricMap *theMap, const mrpt::poses::CPose3D *robotPose=NULL) const
Insert all the observations in this SF into a metric map or any kind (see mrpt::maps::CMetricMap).
Definition: CSensoryFrame.cpp:317
mrpt::poses::CPosePDFGaussian::copyFrom
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Definition: CPosePDFGaussian.cpp:128


Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019