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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ce444d842 Fri Dec 6 19:35:10 2019 +0100 at vie dic 6 19:45:12 CET 2019