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>DEG2RAD(3)) / 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/DEG2RAD(20));
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(DEG2RAD(0.1)));
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, DEG2RAD(180), 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:764
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
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
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
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
double x
X,Y,Z, coords.
Definition: TPose3D.h:31
void clear()
Clear all the gaussian modes.
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
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:33
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: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.
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 ...
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
__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 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:133
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
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:84
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...
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:255
The ICP algorithm return information.
Definition: CICP.h:190
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
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
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:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019