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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: bef67d9c5 Mon Apr 15 00:03:11 2019 +0200 at lun abr 15 00:10:13 CEST 2019