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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019