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


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 63ea9d1f1 Thu Nov 23 00:06:53 2017 +0100 at mar 26 may 2026 12:19:29 CEST