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


Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at mié 12 jul 2023 10:03:34 CEST