MRPT  2.0.4
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-2020, 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};
127  int64_t beaconID;
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 
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 
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), 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 = std::make_unique<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  pdfGrid.reset();
815 
816  } while (repeatGridCalculation);
817 
818  // newDrawnPosition has the pose:
819  }
820 
821  // 3. Get the new "finalPose" of this particle as a random sample
822  // from the
823  // optimal proposal:
824  // ---------------------------------------------------------------------------
825  if (!beacMap->size())
826  {
827  // If we are in the first iteration (no map yet!) just stay
828  // still:
829  finalPose = ith_last_pose;
830  }
831  else
832  {
833  cout << "Drawn: " << newDrawnPosition << endl;
834  // cout << "Final cov was:\n" <<
835  // fusedObsModels.getEstimatedCovariance() << endl << endl;
836 
837  // Make sure it was initialized
838  ASSERT_(
839  firstEstimateRobotHeading !=
840  std::numeric_limits<float>::max());
841 
842  finalPose.setFromValues(
843  newDrawnPosition.x(), newDrawnPosition.y(),
844  newDrawnPosition.z(), firstEstimateRobotHeading, 0, 0);
845  }
846  /** \todo If there are 2+ sensors on the robot, compute phi?
847  */
848 
849  // 4. Update the weight:
850  // In optimal sampling this is the expectation of the
851  // observation likelihood: This is the observation likelihood
852  // averaged
853  // over the whole optimal proposal.
854  // ---------------------------------------------------------------------------
855  double weightUpdate = 0;
856  partIt->log_w += PF_options.powFactor * weightUpdate;
857  }
858  else
859  {
860  // By default:
861  // Generate gaussian-distributed 2D-pose increments according to
862  // mean-cov:
863  if (!robotActionSampler.isPrepared())
865  "Action list does not contain any CActionRobotMovement2D "
866  "or CActionRobotMovement3D object!");
867 
868  robotActionSampler.drawSample(incrPose);
869 
870  finalPose = ith_last_pose + incrPose;
871  }
872 
873  // Insert as the new pose in the path:
874  partIt->d->robotPath.push_back(finalPose.asTPose());
875 
876  // ----------------------------------------------------------------------
877  // UPDATE STAGE
878  // ----------------------------------------------------------------------
879  if (!updateStageAlreadyDone)
880  {
881  partIt->log_w += PF_options.powFactor *
882  (PF_SLAM_computeObservationLikelihoodForParticle(
883  PF_options, i, *sf, finalPose) +
884  extra_log_lik);
885  } // if update not already done...
886 
887  } // end of for each particle "i" & "partIt"
888 
889  MRPT_LOG_DEBUG("Stage 1) Prediction done.");
890 
891  MRPT_END
892 }
893 
894 /*---------------------------------------------------------------
895  prediction_and_update_pfStandardProposal
896  ---------------------------------------------------------------*/
897 void CMultiMetricMapPDF::prediction_and_update_pfStandardProposal(
898  const mrpt::obs::CActionCollection* actions,
899  const mrpt::obs::CSensoryFrame* sf,
901 {
902  MRPT_START
903 
904  PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(
905  actions, sf, PF_options, options.KLD_params);
906 
907  // Average map will need to be updated after this:
908  averageMapIsUpdated = false;
909 
910  MRPT_END
911 }
912 
913 // Specialization for my kind of particles:
914 void CMultiMetricMapPDF::
915  PF_SLAM_implementation_custom_update_particle_with_new_pose(
916  CRBPFParticleData* particleData, const TPose3D& newPose) const
917 {
918  particleData->robotPath.push_back(newPose);
919 }
920 
921 // Specialization for RBPF maps:
922 bool CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations(
923  const CMultiMetricMapPDF::CParticleList& particles,
924  const CSensoryFrame* sf) const
925 {
926  if (sf == nullptr) return false;
927  ASSERT_(!particles.empty());
928  return particles.begin()
929  ->d.get()
930  ->mapTillNow.canComputeObservationsLikelihood(*sf);
931 }
932 
933 /** Do not move the particles until the map is populated. */
934 bool CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement() const
935 {
936  return 0 == getNumberOfObservationsInSimplemap();
937 }
938 
939 /*---------------------------------------------------------------
940  Evaluate the observation likelihood for one
941  particle at a given location
942  ---------------------------------------------------------------*/
943 double CMultiMetricMapPDF::PF_SLAM_computeObservationLikelihoodForParticle(
944  [[maybe_unused]] const CParticleFilter::TParticleFilterOptions& PF_options,
945  const size_t particleIndexForMap, const CSensoryFrame& observation,
946  const CPose3D& x) const
947 {
948  auto* map = const_cast<CMultiMetricMap*>(
949  &m_particles[particleIndexForMap].d->mapTillNow);
950  double ret = 0;
951  for (const auto& it : observation)
952  ret += map->computeObservationLikelihood(*it, x);
953  return ret;
954 }
mrpt::slam::CMetricMapsAlignmentAlgorithm::Align
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
The method for aligning a pair of metric maps, for SE(2) relative poses.
Definition: CMetricMapsAlignmentAlgorithm.cpp:22
mrpt::slam::KLF_loadBinFromParticle
void KLF_loadBinFromParticle(detail::TPathBin2D &outBin, const TKLDParams &opts, const mrpt::maps::CRBPFParticleData *currentParticleValue, const TPose3D *newPoseToBeInserted)
Fills out a "TPathBin2D" variable, given a path hypotesis and (if not set to nullptr) a new pose appe...
Definition: CMultiMetricMapPDF_RBPF.cpp:73
COccupancyGridMap2D.h
mrpt::poses::CPose3DPDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Definition: CPose3DPDFGaussian.h:39
mrpt::slam::CICP::TReturnInfo::goodness
double goodness
A goodness measure for the alignment, it is a [0,1] range indicator of percentage of correspondences.
Definition: CICP.h:200
mrpt::obs::CActionCollection::getActionByClass
T::Ptr getActionByClass(size_t ith=0) const
Access to the i'th action of a given class, or a nullptr smart pointer if there is no action of that ...
Definition: CActionCollection.h:129
CBeaconMap.h
mrpt::poses::CPointPDFGaussian::cov
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
Definition: CPointPDFGaussian.h:44
mrpt::maps::CRBPFParticleData
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
Definition: CMultiMetricMapPDF.h:35
mrpt::poses::CPose3D::pitch
double pitch() const
Get the PITCH angle (in radians)
Definition: CPose3D.h:552
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
TAuxRangeMeasInfo::cmp_Asc
static bool cmp_Asc(const TAuxRangeMeasInfo &a, const TAuxRangeMeasInfo &b)
Auxiliary for optimal sampling in RO-SLAM.
Definition: CMultiMetricMapPDF_RBPF.cpp:132
TAuxRangeMeasInfo::sensedDistance
float sensedDistance
Definition: CMultiMetricMapPDF_RBPF.cpp:126
mrpt::poses::CPointPDFSOG::getMean
void getMean(CPoint3D &mean_point) const override
Definition: CPointPDFSOG.cpp:49
mrpt::poses::CPointPDFSOG::resize
void resize(const size_t N)
Resize the number of SOG modes.
Definition: CPointPDFSOG.cpp:43
mrpt::poses::CPointPDFSOG::clear
void clear()
Clear all the gaussian modes.
Definition: CPointPDFSOG.cpp:39
mrpt::math::CMatrixDynamic::setSize
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
Definition: CMatrixDynamic.h:352
mrpt::poses::CPointPDFSOG::get
const TGaussianMode & get(size_t i) const
Access to individual beacons.
Definition: CPointPDFSOG.h:86
mrpt::poses::CPoseRandomSampler::setPosePDF
void setPosePDF(const CPosePDF &pdf)
This method must be called to select the PDF from which to draw samples.
Definition: CPoseRandomSampler.cpp:103
mrpt::poses::CPoseOrPoint::x
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
mrpt::maps::CPointsMap::TInsertionOptions::minDistBetweenLaserPoints
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:235
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:17
mrpt::poses::CPosePDFGaussian::copyFrom
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPosePDFGaussian.cpp:124
CPosePDFGrid.h
mrpt::poses::CPose3D::setFromValues
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
Definition: CPose3D.cpp:265
IS_CLASS
#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
mrpt::obs::CSensoryFrame::insertObservationsInto
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).
Definition: CSensoryFrame.cpp:266
mrpt::poses::CPoseRandomSampler::isPrepared
bool isPrepared() const
Return true if samples can be generated, which only requires a previous call to setPosePDF.
Definition: CPoseRandomSampler.cpp:338
mrpt::poses::CPointPDFSOG::TGaussianMode::val
CPointPDFGaussian val
Definition: CPointPDFSOG.h:43
mrpt::obs::CActionRobotMovement3D
Represents a probabilistic 3D (6D) movement.
Definition: CActionRobotMovement3D.h:26
CMultiMetricMapPDF.h
mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
Definition: CParticleFilter.h:133
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: CActionCollection.h:26
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
PF_implementations.h
mrpt::math::TPose3D::y
double y
Definition: TPose3D.h:32
MRPT_LOG_WARN_FMT
#define MRPT_LOG_WARN_FMT(_FMT_STRING,...)
Definition: system/COutputLogger.h:465
mrpt::slam::detail::TPoseBin2D::y
int y
Definition: PF_aux_structs.h:27
mrpt::obs::CActionRobotMovement3D::Ptr
std::shared_ptr< mrpt::obs ::CActionRobotMovement3D > Ptr
Definition: CActionRobotMovement3D.h:28
mrpt::poses::CPoseRandomSampler::drawSample
CPose2D & drawSample(CPose2D &p) const
Generate a new sample from the selected PDF.
Definition: CPoseRandomSampler.cpp:185
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
TAuxRangeMeasInfo
Auxiliary for optimal sampling in RO-SLAM.
Definition: CMultiMetricMapPDF_RBPF.cpp:115
mrpt::poses::CPointPDFSOG::end
iterator end()
Definition: CPointPDFSOG.h:101
MRPT_LOG_DEBUG_FMT
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
Definition: system/COutputLogger.h:461
mrpt::obs::CActionRobotMovement2D::Ptr
std::shared_ptr< mrpt::obs ::CActionRobotMovement2D > Ptr
Definition: CActionRobotMovement2D.h:32
mrpt::poses::CPointPDFSOG::ESS
double ESS() const
Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights o...
Definition: CPointPDFSOG.cpp:424
TAuxRangeMeasInfo::nGaussiansInMap
size_t nGaussiansInMap
Definition: CMultiMetricMapPDF_RBPF.cpp:128
CActionCollection.h
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::slam::CICP::TReturnInfo
The ICP algorithm return information.
Definition: CICP.h:190
random.h
mrpt::slam::detail::TPathBin2D
Auxiliary structure
Definition: PF_aux_structs.h:44
mrpt::maps::CRBPFParticleData::robotPath
std::deque< mrpt::math::TPose3D > robotPath
Definition: CMultiMetricMapPDF.h:46
mrpt::poses::CPosePDFGaussian::mean
CPose2D mean
The mean value.
Definition: CPosePDFGaussian.h:44
mrpt::poses::CPointPDFSOG::drawSingleSample
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
Definition: CPointPDFSOG.cpp:208
mrpt::maps::CPointsMap::TInsertionOptions::isPlanarMap
bool isPlanarMap
If set to true, only HORIZONTAL (in the XY plane) measurements will be inserted in the map (Default v...
Definition: CPointsMap.h:254
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
mrpt::slam::detail::TPoseBin2D::phi
int phi
Definition: PF_aux_structs.h:27
val
int val
Definition: mrpt_jpeglib.h:957
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
CActionRobotMovement2D.h
CLandmarksMap.h
mrpt::obs::CObservationBeaconRanges
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Definition: CObservationBeaconRanges.h:24
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::poses::CPose2D
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
Definition: CPose2D.h:39
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:120
mrpt::poses::CPointPDFSOG::TGaussianMode::log_w
double log_w
The log-weight.
Definition: CPointPDFSOG.h:47
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::poses::CPose3D::asTPose
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:759
CActionRobotMovement3D.h
mrpt::poses::CPoseOrPoint::y
double y() const
Definition: CPoseOrPoint.h:147
mrpt::slam::TKLDParams::KLD_binSize_PHI
double KLD_binSize_PHI
Definition: TKLDParams.h:31
mrpt::poses::CPose3D::roll
double roll() const
Get the ROLL angle (in radians)
Definition: CPose3D.h:558
mrpt::maps::CSimplePointsMap
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Definition: CSimplePointsMap.h:30
mrpt::math::TPose3D::x
double x
X,Y,Z, coords.
Definition: TPose3D.h:32
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::slam::detail::TPoseBin2D
Auxiliary structure used in KLD-sampling in particle filters.
Definition: PF_aux_structs.h:23
mrpt::keep_max
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value.
Definition: core/include/mrpt/core/bits_math.h:152
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::slam::detail::TPoseBin2D::x
int x
Bin indices.
Definition: PF_aux_structs.h:27
mrpt::slam::CICP
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
mrpt::poses::CPointPDFSOG::iterator
std::deque< TGaussianMode >::iterator iterator
Definition: CPointPDFSOG.h:52
mrpt::random::CRandomGenerator::drawGaussianMultivariate
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.
Definition: RandomGenerators.h:255
utils.h
MRPT_LOG_WARN
#define MRPT_LOG_WARN(_STRING)
Definition: system/COutputLogger.h:431
mrpt::math::CVectorDynamic< double >
TAuxRangeMeasInfo::beaconID
int64_t beaconID
Definition: CMultiMetricMapPDF_RBPF.cpp:127
mrpt::slam::detail::TPathBin2D::bins
std::vector< TPoseBin2D > bins
Definition: PF_aux_structs.h:46
mrpt::poses::CPointPDFSOG::begin
iterator begin()
Definition: CPointPDFSOG.h:100
mrpt::obs::CActionCollection::getBestMovementEstimation
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
Definition: CActionCollection.cpp:94
PF_aux_structs.h
mrpt::poses::CPoseRandomSampler
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
Definition: CPoseRandomSampler.h:42
mrpt::poses::CPoseRandomSampler::getOriginalPDFCov2D
void getOriginalPDFCov2D(mrpt::math::CMatrixDouble33 &cov3x3) const
Retrieves the 3x3 covariance of the original PDF in .
Definition: CPoseRandomSampler.cpp:342
mrpt::poses::CPointPDFSOG
Declares a class that represents a Probability Density function (PDF) of a 3D point .
Definition: CPointPDFSOG.h:33
CObservationBeaconRanges.h
TAuxRangeMeasInfo::sensorLocationOnRobot
CPoint3D sensorLocationOnRobot
Definition: CMultiMetricMapPDF_RBPF.cpp:125
mrpt::math::TPose3D::yaw
double yaw
Yaw coordinate (rotation angle over Z axis).
Definition: TPose3D.h:34
mrpt::maps::CPointsMap::insertionOptions
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:274
mrpt::random::getRandomGenerator
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Definition: RandomGenerator.cpp:89
CTicTac.h
CPosePDFGaussian.h
slam-precomp.h
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:30
mrpt::poses::CPose3D::yaw
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:546
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::CBeaconMap
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian,...
Definition: CBeaconMap.h:43
mrpt::poses::CPointPDFSOG::erase
iterator erase(iterator i)
Definition: CPointPDFSOG.h:104
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::poses::CPose3DPDFGaussian::cov
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
Definition: CPose3DPDFGaussian.h:82
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
mrpt::maps::CLandmarksMap
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
mrpt::maps::CMetricMap::computeObservationLikelihood
double computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom)
Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.
Definition: CMetricMap.cpp:170
mrpt::random
A namespace of pseudo-random numbers generators of diferent distributions.
Definition: random_shuffle.h:18
mrpt::math::CProbabilityDensityFunction::getCovariance
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
Definition: CProbabilityDensityFunction.h:88
mrpt::poses::CPointPDFSOG::bayesianFusion
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),...
Definition: CPointPDFSOG.cpp:249
mrpt::maps
Definition: CBeacon.h:21
mrpt::poses::CPosePDFGaussian
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
Definition: CPosePDFGaussian.h:28
CFileStream.h
mrpt::poses::CPoint3D
A class used to store a 3D point.
Definition: CPoint3D.h:31
CSimplePointsMap.h
mrpt::poses::CPointPDFSOG::TGaussianMode
The struct for each mode:
Definition: CPointPDFSOG.h:40
mrpt::poses::CPointPDFGaussian::mean
CPoint3D mean
The mean value.
Definition: CPointPDFGaussian.h:42
mrpt::poses::CPosePDF::Ptr
std::shared_ptr< CPosePDF > Ptr
Definition: CPosePDF.h:41
mrpt::slam
Definition: CMultiMetricMapPDF.h:26
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:102
mrpt::poses::CPose3DPDFGaussian::mean
CPose3D mean
The mean value.
Definition: CPose3DPDFGaussian.h:78
mrpt::math::CMatrixDynamic< double >
mrpt::slam::TKLDParams::KLD_binSize_XY
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:31
INVALID_BEACON_ID
#define INVALID_BEACON_ID
Used for CObservationBeaconRange, CBeacon, etc.
Definition: CObservation.h:23
mrpt::poses::CPointPDFSOG::size
size_t size() const
Return the number of Gaussian modes.
Definition: CPointPDFSOG.h:108
TAuxRangeMeasInfo::TAuxRangeMeasInfo
TAuxRangeMeasInfo()
Definition: CMultiMetricMapPDF_RBPF.cpp:117
mrpt::poses::CPointPDFSOG::push_back
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
Definition: CPointPDFSOG.h:99



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Fri Jul 17 08:43:33 UTC 2020