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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019