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 
26 #include <mrpt/math.h>
27 
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 <>
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 <>
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:
109 
110 /** Auxiliary for optimal sampling in RO-SLAM */
112 {
114  sensorLocationOnRobot(),
115  sensedDistance(0),
116  beaconID(INVALID_BEACON_ID),
117  nGaussiansInMap(0)
118  {}
119 
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,
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,
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,
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 
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());
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  {
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 
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 
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,
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 
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).
GLboolean GLboolean GLboolean GLboolean a
Definition: glew.h:5406
void drawSingleSample(CPoint3D &outSample) const MRPT_OVERRIDE
Draw a sample from the pdf.
CPose2D mean
The mean value.
CPose3D mean
The mean value.
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:30
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.
CPose2D & drawSample(CPose2D &p) const
Generate a new sample from the selected PDF.
static bool cmp_Asc(const TAuxRangeMeasInfo &a, const TAuxRangeMeasInfo &b)
Auxiliary for optimal sampling in RO-SLAM.
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
CPoint3D mean
The mean value.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:52
void resize(const size_t N)
Resize the number of SOG modes.
const GLfloat * c
Definition: glew.h:10088
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:35
#define THROW_EXCEPTION(msg)
The struct for each mode:
Definition: CPointPDFSOG.h:43
BASE_IMPEXP CRandomGenerator randomGenerator
A static instance of a CRandomGenerator class, for use in single-thread applications.
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
Scalar * iterator
Definition: eigen_plugins.h:23
void uniformDistribution()
Assigns the same value to all the cells in the grid, so the sum 1.
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
void clear()
Clear all the gaussian modes.
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Option set for KLD algorithm.
Definition: TKLDParams.h:22
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
T::Ptr getActionByClass(const size_t &ith=0) const
Access to the i'th action of a given class, or a NULL smart pointer if there is no action of that cla...
bool isPrepared() const
Return true if samples can be generated, which only requires a previous call to setPosePDF.
std::deque< CBeacon >::iterator iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
double yaw
Yaw coordinate (rotation angle over Z axis).
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...
Declares a class for storing a collection of robot actions.
std::deque< CObservationPtr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
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...
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:391
Represents a probabilistic 3D (6D) movement.
float goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences...
Definition: CICP.h:137
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
Auxiliary structure.
A class for storing a map of 3D probabilistic landmarks.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_END
void setPosePDF(const CPosePDF *pdf)
This method must be called to select the PDF from which to draw samples.
const T * getByIndex(size_t x, size_t y, size_t phi) const
Reads the contents of a cell.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
__int64 int64_t
Definition: rptypes.h:51
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
Definition: CPointPDFSOG.h:99
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
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
std::deque< TMeasurement > sensedData
The list of observed ranges.
void getMean(CPoint3D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF) ...
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
double idx2x(size_t x) const
Returns coordinates from "indexes":
double KLD_binSize_XY
Parameters for the KLD adaptive sample size algorithm (see Dieter Fox's papers), which is used only i...
Definition: TKLDParams.h:32
#define DEG2RAD
GLfloat GLfloat p
Definition: glew.h:10113
CActionRobotMovement2DPtr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
A class used to store a 3D point.
Definition: CPoint3D.h:32
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
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
iterator erase(iterator i)
Definition: CPointPDFSOG.h:108
size_t size() const
Return the number of Gaussian modes.
Definition: CPointPDFSOG.h:111
Auxiliary structure used in KLD-sampling in particle filters.
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
#define INVALID_BEACON_ID
Used for CObservationBeaconRange, CBeacon, etc.
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!)
#define MRPT_START
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
Declares a virtual base class for all metric maps storage classes.
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's observation.
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
#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
The configuration of a particle filter.
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:174
std::deque< mrpt::math::TPose3D > robotPath
double ESS() const
Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights o...
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
The ICP algorithm return information.
Definition: CICP.h:128
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
#define ASSERT_(f)
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
std::vector< TPoseBin2D > bins
void getOriginalPDFCov2D(mrpt::math::CMatrixDouble33 &cov3x3) const
Retrieves the 3x3 covariance of the original PDF in .
GLuint GLfloat * val
Definition: glew.h:7785
Auxiliary for optimal sampling in RO-SLAM.
std::deque< TGaussianMode >::iterator iterator
Definition: CPointPDFSOG.h:58
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
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.
This class stores any customizable set of metric maps.
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x...
Definition: CPosePDFGrid.h:30
double idx2y(size_t y) const
Returns coordinates from "indexes":
void normalize()
Normalizes the PDF, such as all cells sum the unity.
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018