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  size_t M = m_particles.size();
200  bool updateStageAlreadyDone = false;
201  CPose3D initialPose, incrPose, finalPose;
202 
203  // ICP used if "pfOptimalProposal_mapSelection" = 0 or 1
204  CICP icp(
205  options.icp_params); // Set our ICP params instead of default ones.
206  CICP::TReturnInfo icpInfo;
207 
208  CParticleList::iterator partIt;
209 
210  ASSERT_(sf != nullptr);
211  // Find a robot movement estimation:
212  CPose3D motionModelMeanIncr; // The mean motion increment:
213  CPoseRandomSampler robotActionSampler;
214  {
215  CActionRobotMovement2D::Ptr robotMovement2D =
216  actions->getBestMovementEstimation();
217 
218  // If there is no 2D action, look for a 3D action:
219  if (robotMovement2D)
220  {
221  robotActionSampler.setPosePDF(*robotMovement2D->poseChange);
222  motionModelMeanIncr =
223  mrpt::poses::CPose3D(robotMovement2D->poseChange->getMeanVal());
224  }
225  else
226  {
227  CActionRobotMovement3D::Ptr robotMovement3D =
229  if (robotMovement3D)
230  {
231  robotActionSampler.setPosePDF(robotMovement3D->poseChange);
232  robotMovement3D->poseChange.getMean(motionModelMeanIncr);
233  }
234  else
235  {
236  motionModelMeanIncr.setFromValues(0, 0, 0);
237  }
238  }
239  }
240 
241  // Average map will need to be updated after this:
242  averageMapIsUpdated = false;
243 
244  // --------------------------------------------------------------------------------------
245  // Prediction:
246  //
247  // Compute a new mean and covariance by sampling around the mean of the
248  // input "action"
249  // --------------------------------------------------------------------------------------
250  printf(" 1) Prediction...");
251  M = m_particles.size();
252 
253  // To be computed as an average from all m_particles:
254  size_t particleWithHighestW = 0;
255  for (size_t i = 0; i < M; i++)
256  if (getW(i) > getW(particleWithHighestW)) particleWithHighestW = i;
257 
258  // The paths MUST already contain the starting location for each particle:
259  ASSERT_(!m_particles[0].d->robotPath.empty());
260  // Build the local map of points for ICP:
261  CSimplePointsMap localMapPoints;
262  CLandmarksMap localMapLandmarks;
263  bool built_map_points = false;
264  bool built_map_lms = false;
265 
266  // Update particle poses:
267  size_t i;
268  for (i = 0, partIt = m_particles.begin(); partIt != m_particles.end();
269  partIt++, i++)
270  {
271  double extra_log_lik = 0; // Used for the optimal_PF with ICP
272 
273  // Set initial robot pose estimation for this particle:
274  const CPose3D ith_last_pose = CPose3D(
275  *partIt->d->robotPath.rbegin()); // The last robot pose in the path
276 
277  CPose3D initialPoseEstimation = ith_last_pose + motionModelMeanIncr;
278 
279  // Use ICP with the map associated to particle?
280  if (options.pfOptimalProposal_mapSelection == 0 ||
281  options.pfOptimalProposal_mapSelection == 1 ||
282  options.pfOptimalProposal_mapSelection == 3)
283  {
284  CPosePDFGaussian icpEstimation;
285 
286  // Configure the matchings that will take place in the ICP process:
287  auto partMap = partIt->d->mapTillNow;
288  const auto numPtMaps = partMap.countMapsByClass<CSimplePointsMap>();
289 
290  ASSERT_(numPtMaps == 0 || numPtMaps == 1);
291 
292  CMetricMap* map_to_align_to = nullptr;
293 
294  if (options.pfOptimalProposal_mapSelection == 0) // Grid map
295  {
296  auto grid = partMap.mapByClass<COccupancyGridMap2D>();
297  ASSERT_(grid);
298 
299  // Build local map of points.
300  if (!built_map_points)
301  {
302  built_map_points = true;
303 
304  localMapPoints.insertionOptions.minDistBetweenLaserPoints =
305  0.02f;
306  localMapPoints.insertionOptions.isPlanarMap = true;
307  sf->insertObservationsInto(&localMapPoints);
308  }
309 
310  map_to_align_to = grid.get();
311  }
312  // Map of points
313  else if (options.pfOptimalProposal_mapSelection == 3)
314  {
315  auto ptsMap = partMap.mapByClass<CSimplePointsMap>();
316  ASSERT_(ptsMap);
317 
318  // Build local map of points.
319  if (!built_map_points)
320  {
321  built_map_points = true;
322 
323  localMapPoints.insertionOptions.minDistBetweenLaserPoints =
324  0.02f;
325  localMapPoints.insertionOptions.isPlanarMap = true;
326  sf->insertObservationsInto(&localMapPoints);
327  }
328 
329  map_to_align_to = ptsMap.get();
330  }
331  else
332  {
333  auto lmMap = partMap.mapByClass<CLandmarksMap>();
334  ASSERT_(lmMap);
335 
336  // Build local map of LMs.
337  if (!built_map_lms)
338  {
339  built_map_lms = true;
340  sf->insertObservationsInto(&localMapLandmarks);
341  }
342 
343  map_to_align_to = lmMap.get();
344  }
345 
346  ASSERT_(map_to_align_to != nullptr);
347 
348  // Use ICP to align to each particle's map:
349  {
350  CPosePDF::Ptr alignEst = icp.Align(
351  map_to_align_to, &localMapPoints,
352  CPose2D(initialPoseEstimation), nullptr, &icpInfo);
353  icpEstimation.copyFrom(*alignEst);
354  }
355 
356  if (i == particleWithHighestW)
357  {
358  newInfoIndex = 1 - icpInfo.goodness; // newStaticPointsRatio;
359  // //* icpInfo.goodness;
360  }
361 
362  // Set the gaussian pose:
363  CPose3DPDFGaussian finalEstimatedPoseGauss(icpEstimation);
364 
365  // printf("[rbpf-slam] gridICP[%u]: %.02f%%\n", i,
366  // 100*icpInfo.goodness);
367  if (icpInfo.goodness < options.ICPGlobalAlign_MinQuality &&
368  SFs.size())
369  {
370  printf(
371  "[rbpf-slam] Warning: gridICP[%u]: %.02f%% -> Using "
372  "odometry instead!\n",
373  (unsigned int)i, 100 * icpInfo.goodness);
374  icpEstimation.mean = CPose2D(initialPoseEstimation);
375  }
376 
377  // As a way to take into account the odometry / "prior", use
378  // a correcting factor in the likelihood from the mismatch
379  // prior<->icp_estimate:
380  // const double prior_dist_lin =
381  // initialPoseEstimation.distanceTo(icpEstimation.mean);
382  // const double prior_dist_ang = std::abs(
383  // mrpt::math::wrapToPi(
384  // initialPoseEstimation.yaw()-icpEstimation.mean.phi() ) );
385  //// if (prior_dist_lin>0.10 ||
386  /// prior_dist_ang>DEG2RAD(3)) / printf(" >>>>>>>>>>
387  /// %f %f\n",prior_dist_lin,RAD2DEG(prior_dist_ang));
388  // extra_log_lik = -(prior_dist_lin/0.20) -
389  //(prior_dist_ang/DEG2RAD(20));
390 
391  // printf("gICP: %.02f%%,
392  // Iters=%u\n",icpInfo.goodness,icpInfo.nIterations);
393 
394 #if 0 // Use hacked ICP covariance:
395  CPose3D Ap = finalEstimatedPoseGauss.mean - ith_last_pose;
396  const double Ap_dist = Ap.norm();
397 
398  finalEstimatedPoseGauss.cov.setZero();
399  finalEstimatedPoseGauss.cov(0,0) = square( fabs(Ap_dist)*0.01 );
400  finalEstimatedPoseGauss.cov(1,1) = square( fabs(Ap_dist)*0.01 );
401  finalEstimatedPoseGauss.cov(2,2) = square( fabs(Ap.yaw())*0.02 );
402 #else
403  // Use real ICP covariance (with a minimum level):
404  keep_max(finalEstimatedPoseGauss.cov(0, 0), square(0.002));
405  keep_max(finalEstimatedPoseGauss.cov(1, 1), square(0.002));
406  keep_max(finalEstimatedPoseGauss.cov(2, 2), square(DEG2RAD(0.1)));
407 
408 #endif
409 
410  // Generate gaussian-distributed 2D-pose increments according to
411  // "finalEstimatedPoseGauss":
412  // -------------------------------------------------------------------------------------------
413  finalPose =
414  finalEstimatedPoseGauss.mean; // Add to the new robot pose:
416  rndSamples, finalEstimatedPoseGauss.cov);
417  // Add noise:
418  finalPose.setFromValues(
419  finalPose.x() + rndSamples[0], finalPose.y() + rndSamples[1],
420  finalPose.z(), finalPose.yaw() + rndSamples[2],
421  finalPose.pitch(), finalPose.roll());
422  }
423  else if (options.pfOptimalProposal_mapSelection == 2)
424  {
425  // Perform optimal sampling with the beacon map:
426  // Described in paper: IROS 2008
427  // --------------------------------------------------------
428  auto beacMap = partIt->d->mapTillNow.mapByClass<CBeaconMap>();
429 
430  // We'll also update the weight of the particle here
431  updateStageAlreadyDone = true;
432 
433  // =====================================================================
434  // SUMMARY:
435  // For each beacon measurement in the SF, stored in
436  // "lstObservedRanges",
437  // compute the SOG of the observation model, and multiply all of
438  // them
439  // (fuse) in "fusedObsModels". The result will hopefully be a very
440  // small
441  // PDF where to draw a sample from for the new robot pose.
442  // =====================================================================
443  bool methodSOGorGrid = false; // TRUE=SOG
444  CPoint3D newDrawnPosition;
445  float firstEstimateRobotHeading = std::numeric_limits<float>::max();
446 
447  // The parameters to discard too far gaussians:
448  CPoint3D centerPositionPrior(ith_last_pose);
449  float centerPositionPriorRadius = 2.0f;
450 
451  if (!robotActionSampler.isPrepared())
452  {
453  firstEstimateRobotHeading = ith_last_pose.yaw();
454  // If the map is empty: There is no solution!:
455  // THROW_EXCEPTION("There is no odometry & the initial beacon
456  // map is empty: RO-SLAM has no solution -> ABORTED!!!");
457 
458  if (!beacMap->size())
459  {
460  // First iteration only...
461  cerr << "[RO-SLAM] Optimal filtering without map & "
462  "odometry...this message should appear only the "
463  "first iteration!!"
464  << endl;
465  }
466  else
467  {
468  // To make RO-SLAM to have a solution, arbitrarily fix one
469  // of the beacons so
470  // unbiguity dissapears.
471  if (beacMap->get(0).m_typePDF == CBeacon::pdfSOG)
472  {
473  cerr << "[RO-SLAM] Optimal filtering without map & "
474  "odometry->FIXING ONE BEACON!"
475  << endl;
476  ASSERT_(beacMap->get(0).m_locationSOG.size() > 0);
477  CPoint3D fixedBeacon(
478  beacMap->get(0).m_locationSOG[0].val.mean);
479 
480  // Pass to gaussian without uncertainty:
481  beacMap->get(0).m_typePDF = CBeacon::pdfGauss;
482  beacMap->get(0).m_locationSOG.clear();
483  beacMap->get(0).m_locationGauss.mean = fixedBeacon;
484  beacMap->get(0).m_locationGauss.cov.setDiagonal(
485  3, 1e-6);
486  }
487  }
488  } // end if there is no odometry
489 
490  // 1. Make the list of beacon IDs-ranges:
491  // -------------------------------------------
492  deque<TAuxRangeMeasInfo> lstObservedRanges;
493 
494  for (const auto& itObs : *sf)
495  {
496  if (IS_CLASS(*itObs, CObservationBeaconRanges))
497  {
498  const auto* obs =
499  static_cast<const CObservationBeaconRanges*>(
500  itObs.get());
501  deque<CObservationBeaconRanges::TMeasurement>::
502  const_iterator itRanges;
503  for (itRanges = obs->sensedData.begin();
504  itRanges != obs->sensedData.end(); itRanges++)
505  {
506  ASSERT_(itRanges->beaconID != INVALID_BEACON_ID);
507  // only add those in the map:
508  for (auto itBeacs = beacMap->begin();
509  itBeacs != beacMap->end(); ++itBeacs)
510  {
511  if ((itBeacs)->m_ID == itRanges->beaconID)
512  {
513  TAuxRangeMeasInfo newMeas;
514  newMeas.beaconID = itRanges->beaconID;
515  newMeas.sensedDistance =
516  itRanges->sensedDistance;
517  newMeas.sensorLocationOnRobot =
518  itRanges->sensorLocationOnRobot;
519 
520  ASSERT_(
521  (itBeacs)->m_typePDF == CBeacon::pdfGauss ||
522  (itBeacs)->m_typePDF == CBeacon::pdfSOG);
523  newMeas.nGaussiansInMap =
524  (itBeacs)->m_typePDF == CBeacon::pdfSOG
525  ? (itBeacs)->m_locationSOG.size()
526  : 1 /*pdfGauss*/;
527 
528  lstObservedRanges.push_back(newMeas);
529  break; // Next observation
530  }
531  }
532  }
533  }
534  }
535 
536  // ASSERT_( lstObservedRanges.size()>=2 );
537 
538  // Sort ascending ranges: Smallest ranges first -> the problem is
539  // easiest!
540  sort(
541  lstObservedRanges.begin(), lstObservedRanges.end(),
543 
544  if (methodSOGorGrid)
545  {
546  CPointPDFSOG fusedObsModels; //<- p(z_t|x_t) for all the range
547  // measurements (fused)
548  fusedObsModels.clear();
549 
550  // 0. OPTIONAL: Create a "prior" as a first mode in
551  // "fusedObsModels"
552  // using odometry. If there is no odometry, we
553  // *absolutely* need
554  // at least one well-localized beacon at the beginning, or
555  // the symmetry cannot be broken!
556  // -----------------------------------------------------------------------------------------------
557  if (robotActionSampler.isPrepared())
558  {
560  newMode.log_w = 0;
561 
562  CPose3D auxPose =
563  ith_last_pose +
564  motionModelMeanIncr; // CPose3D(robotMovement->poseChange->getEstimatedPose()));
565  firstEstimateRobotHeading = auxPose.yaw();
566 
567  newMode.val.mean = CPoint3D(auxPose);
568 
569  // Uncertainty in z is null:
570  // CMatrixF poseCOV =
571  // robotMovement->poseChange->getEstimatedCovariance();
572  CMatrixD poseCOV;
573  robotActionSampler.getOriginalPDFCov2D(poseCOV);
574 
575  poseCOV.setSize(2, 2);
576  poseCOV.setSize(3, 3);
577  newMode.val.cov = poseCOV;
578  fusedObsModels.push_back(newMode); // Add it:
579  }
580 
581  // 2. Generate the optimal proposal by fusing obs models
582  // -------------------------------------------------------------
583  for (auto itBeacs = beacMap->begin(); itBeacs != beacMap->end();
584  ++itBeacs)
585  {
586  // for each observed beacon (by its ID), generate
587  // observation model:
588  for (auto& lstObservedRange : lstObservedRanges)
589  {
590  if ((itBeacs)->m_ID == lstObservedRange.beaconID)
591  {
592  // Match:
593  float sensedRange = lstObservedRange.sensedDistance;
594 
595  CPointPDFSOG newObsModel;
596  (itBeacs)->generateObservationModelDistribution(
597  sensedRange, newObsModel,
598  beacMap.get(), // The beacon map, for options
599  lstObservedRange
600  .sensorLocationOnRobot, // Sensor
601  // location on
602  // robot
603  centerPositionPrior, centerPositionPriorRadius);
604 
605  if (!fusedObsModels.size())
606  {
607  // This is the first one: Just copy the obs.
608  // model here:
609  fusedObsModels = newObsModel;
610  }
611  else
612  {
613  // Fuse with existing:
614  CPointPDFSOG tempFused(0);
615  tempFused.bayesianFusion(
616  fusedObsModels, newObsModel,
617  3 // minMahalanobisDistToDrop
618  );
619  fusedObsModels = tempFused;
620  }
621 
622  // Remove modes with negligible weights:
623  // -----------------------------------------------------------
624 
625  {
626  cout << "#modes bef: " << fusedObsModels.size()
627  << " ESS: " << fusedObsModels.ESS()
628  << endl;
629  double max_w = -1e100;
630  // int idx;
631 
633 
634  for (it = fusedObsModels.begin();
635  it != fusedObsModels.end(); it++)
636  max_w =
637  max(max_w, (it)->log_w); // keep the
638  // maximum
639  // mode weight
640 
641  for (it = fusedObsModels.begin();
642  it != fusedObsModels.end();)
643  {
644  if (max_w - (it)->log_w >
645  20) // Remove the mode:
646  it = fusedObsModels.erase(it);
647  else
648  it++;
649  }
650 
651  cout
652  << "#modes after: " << fusedObsModels.size()
653  << endl;
654  }
655 
656  // Shall we simplify the PDF?
657  // -----------------------------------------------------------
658  CMatrixDouble currentCov;
659  fusedObsModels.getCovariance(currentCov);
660  ASSERT_(
661  currentCov(0, 0) > 0 && currentCov(1, 1) > 0);
662  if (sqrt(currentCov(0, 0)) < 0.10f &&
663  sqrt(currentCov(1, 1)) < 0.10f &&
664  sqrt(currentCov(2, 2)) < 0.10f)
665  {
666  // Approximate by a single gaussian!
667  CPoint3D currentMean;
668  fusedObsModels.getMean(currentMean);
669  fusedObsModels[0].log_w = 0;
670  fusedObsModels[0].val.mean = currentMean;
671  fusedObsModels[0].val.cov = currentCov;
672 
673  fusedObsModels.resize(1);
674  }
675 
676  { /*
677  CMatrixD evalGrid;
678  fusedObsModels.evaluatePDFInArea(-3,3,-3,3,0.1,0,evalGrid,
679  true);
680  evalGrid *= 1.0/evalGrid.maxCoeff();
681  CImage imgF(evalGrid, true);
682  static int autoCount=0;
683  imgF.saveToFile(format("debug_%04i.png",autoCount++));*/
684  }
685  }
686  } // end for itObs (in lstObservedRanges)
687 
688  } // end for itBeacs
689 
690  /** /
691  COpenGLScene scene;
692  opengl::CSetOfObjects *obj = new opengl::CSetOfObjects();
693  fusedObsModels.getAs3DObject( *obj );
694  scene.insert( obj );
695  CFileStream("debug.3Dscene",fomWrite) << scene;
696  cout << "fusedObsModels # of modes: " <<
697  fusedObsModels.m_modes.size() << endl;
698  printf("ESS: %f\n",fusedObsModels.ESS() );
699  cout << fusedObsModels.getEstimatedCovariance() << endl;
700  mrpt::system::pause(); / **/
701 
702  if (beacMap->size())
703  fusedObsModels.drawSingleSample(newDrawnPosition);
704  }
705  else
706  {
707  // =============================
708  // GRID METHOD
709  // =============================
710 
711  float grid_min_x = ith_last_pose.x() - 0.5f;
712  float grid_max_x = ith_last_pose.x() + 0.5f;
713  float grid_min_y = ith_last_pose.y() - 0.5f;
714  float grid_max_y = ith_last_pose.y() + 0.5f;
715  float grid_resXY = 0.02f;
716 
717  bool repeatGridCalculation = false;
718 
719  do
720  {
721  auto* pdfGrid = new CPosePDFGrid(
722  grid_min_x, grid_max_x, grid_min_y, grid_max_y,
723  grid_resXY, DEG2RAD(180), 0, 0);
724 
725  pdfGrid->uniformDistribution();
726 
727  // Fuse all the observation models in the grid:
728  // -----------------------------------------------------
729  for (auto itBeacs = beacMap->begin();
730  itBeacs != beacMap->end(); ++itBeacs)
731  {
732  // for each observed beacon (by its ID), generate
733  // observation model:
734  for (auto& lstObservedRange : lstObservedRanges)
735  {
736  if ((itBeacs)->m_ID == lstObservedRange.beaconID)
737  {
738  // Match:
739  float sensedRange =
740  lstObservedRange.sensedDistance;
741 
742  /** /
743  CPointPDFSOG
744  newObsModel;
745  (itBeacs)->generateObservationModelDistribution(
746  sensedRange,
747  newObsModel,
748  beacMap,
749  // The beacon map, for options
750  itObs->sensorLocationOnRobot,//
751  Sensor location on robot
752  centerPositionPrior,
753  centerPositionPriorRadius
754  );
755  / **/
756  for (size_t idxX = 0;
757  idxX < pdfGrid->getSizeX(); idxX++)
758  {
759  float grid_x = pdfGrid->idx2x(idxX);
760  for (size_t idxY = 0;
761  idxY < pdfGrid->getSizeY(); idxY++)
762  {
763  double grid_y = pdfGrid->idx2y(idxY);
764 
765  // Evaluate obs model:
766  double* cell =
767  pdfGrid->getByIndex(idxX, idxY, 0);
768 
769  //
770  double lik =
771  1; // newObsModel.evaluatePDF(CPoint3D(grid_x,grid_y,0),true);
772  switch ((itBeacs)->m_typePDF)
773  {
774  case CBeacon::pdfSOG:
775  {
776  CPointPDFSOG* sog =
777  &(itBeacs)->m_locationSOG;
778  double sensorSTD2 = square(
779  beacMap->likelihoodOptions
780  .rangeStd);
781 
783  for (it = sog->begin();
784  it != sog->end(); it++)
785  {
786  lik *= exp(
787  -0.5 *
788  square(
789  sensedRange -
790  (it)->val.mean.distance2DTo(
791  grid_x +
792  lstObservedRange
793  .sensorLocationOnRobot
794  .x(),
795  grid_y +
796  lstObservedRange
797  .sensorLocationOnRobot
798  .y())) /
799  sensorSTD2);
800  }
801  }
802  break;
803  default:
804  break; // THROW_EXCEPTION("NO");
805  }
806 
807  (*cell) *= lik;
808 
809  } // for idxY
810  } // for idxX
811  pdfGrid->normalize();
812  } // end if match
813  } // end for itObs
814  } // end for beacons in map
815 
816  // Draw the single pose from the grid:
817  if (beacMap->size())
818  {
819  // Take the most likely cell:
820  float maxW = -1e10f;
821  float maxX = 0, maxY = 0;
822  for (size_t idxX = 0; idxX < pdfGrid->getSizeX();
823  idxX++)
824  {
825  // float grid_x = pdfGrid->idx2x(idxX);
826  for (size_t idxY = 0; idxY < pdfGrid->getSizeY();
827  idxY++)
828  {
829  // float grid_y = pdfGrid->idx2y(idxY);
830 
831  // Evaluate obs model:
832  float c = *pdfGrid->getByIndex(idxX, idxY, 0);
833  if (c > maxW)
834  {
835  maxW = c;
836  maxX = pdfGrid->idx2x(idxX);
837  maxY = pdfGrid->idx2y(idxY);
838  }
839  }
840  }
841  newDrawnPosition.x(maxX);
842  newDrawnPosition.y(maxY);
843 
844 #if 0
845  {
846  //cout << "Grid: " << pdfGaussApprox << endl;
847  //pdfGrid->saveToTextFile("debug.txt");
848  CMatrixDouble outMat;
849  pdfGrid->getAsMatrix(0, outMat );
850  outMat *= 1.0f/outMat.maxCoeff();
851  CImage imgF(outMat, true);
852  static int autocount=0;
853  imgF.saveToFile(format("debug_grid_%f_%05i.png",grid_resXY,autocount++));
854  printf("grid res: %f MAX: %f,%f\n",grid_resXY,maxX,maxY);
855  //mrpt::system::pause();
856  }
857 #endif
858 
859  if (grid_resXY > 0.01f)
860  {
861  grid_min_x = maxX - 0.03f;
862  grid_max_x = maxX + 0.03f;
863  grid_min_y = maxY - 0.03f;
864  grid_max_y = maxY + 0.03f;
865  grid_resXY = 0.002f;
866  repeatGridCalculation = true;
867  }
868  else
869  repeatGridCalculation = false;
870 
871  /*
872  // Approximate by a Gaussian:
873  CPosePDFGaussian pdfGaussApprox(
874  pdfGrid->getEstimatedPose(),
875  pdfGrid->getEstimatedCovariance() );
876  CPose2D newDrawnPose;
877  //pdfGrid->drawSingleSample( newDrawnPose );
878  pdfGaussApprox.drawSingleSample( newDrawnPose );
879  newDrawnPosition = newDrawnPose;
880  */
881  }
882  delete pdfGrid;
883  pdfGrid = nullptr;
884 
885  } while (repeatGridCalculation);
886 
887  // newDrawnPosition has the pose:
888  }
889 
890  // 3. Get the new "finalPose" of this particle as a random sample
891  // from the
892  // optimal proposal:
893  // ---------------------------------------------------------------------------
894  if (!beacMap->size())
895  {
896  // If we are in the first iteration (no map yet!) just stay
897  // still:
898  finalPose = ith_last_pose;
899  }
900  else
901  {
902  cout << "Drawn: " << newDrawnPosition << endl;
903  // cout << "Final cov was:\n" <<
904  // fusedObsModels.getEstimatedCovariance() << endl << endl;
905 
906  // Make sure it was initialized
907  ASSERT_(
908  firstEstimateRobotHeading !=
909  std::numeric_limits<float>::max());
910 
911  finalPose.setFromValues(
912  newDrawnPosition.x(), newDrawnPosition.y(),
913  newDrawnPosition.z(), firstEstimateRobotHeading, 0, 0);
914  }
915  /** \todo If there are 2+ sensors on the robot, compute phi?
916  */
917 
918  // 4. Update the weight:
919  // In optimal sampling this is the expectation of the
920  // observation likelihood: This is the observation likelihood
921  // averaged
922  // over the whole optimal proposal.
923  // ---------------------------------------------------------------------------
924  double weightUpdate = 0;
925  partIt->log_w += PF_options.powFactor * weightUpdate;
926  }
927  else
928  {
929  // By default:
930  // Generate gaussian-distributed 2D-pose increments according to
931  // mean-cov:
932  if (!robotActionSampler.isPrepared())
934  "Action list does not contain any CActionRobotMovement2D "
935  "or CActionRobotMovement3D object!");
936 
937  robotActionSampler.drawSample(incrPose);
938 
939  finalPose = ith_last_pose + incrPose;
940  }
941 
942  // Insert as the new pose in the path:
943  partIt->d->robotPath.push_back(finalPose.asTPose());
944 
945  // ----------------------------------------------------------------------
946  // UPDATE STAGE
947  // ----------------------------------------------------------------------
948  if (!updateStageAlreadyDone)
949  {
950  partIt->log_w += PF_options.powFactor *
951  (PF_SLAM_computeObservationLikelihoodForParticle(
952  PF_options, i, *sf, finalPose) +
953  extra_log_lik);
954  } // if update not already done...
955 
956  } // end of for each particle "i" & "partIt"
957 
958  printf("Ok\n");
959 
960  MRPT_END
961 }
962 
963 /*---------------------------------------------------------------
964  prediction_and_update_pfStandardProposal
965  ---------------------------------------------------------------*/
966 void CMultiMetricMapPDF::prediction_and_update_pfStandardProposal(
967  const mrpt::obs::CActionCollection* actions,
968  const mrpt::obs::CSensoryFrame* sf,
970 {
971  MRPT_START
972 
973  PF_SLAM_implementation_pfStandardProposal<mrpt::slam::detail::TPoseBin2D>(
974  actions, sf, PF_options, options.KLD_params);
975 
976  // Average map will need to be updated after this:
977  averageMapIsUpdated = false;
978 
979  MRPT_END
980 }
981 
982 // Specialization for my kind of particles:
983 void CMultiMetricMapPDF::
984  PF_SLAM_implementation_custom_update_particle_with_new_pose(
985  CRBPFParticleData* particleData, const TPose3D& newPose) const
986 {
987  particleData->robotPath.push_back(newPose);
988 }
989 
990 // Specialization for RBPF maps:
991 bool CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations(
992  const CMultiMetricMapPDF::CParticleList& particles,
993  const CSensoryFrame* sf) const
994 {
995  if (sf == nullptr) return false;
996  ASSERT_(!particles.empty());
997  return particles.begin()
998  ->d.get()
999  ->mapTillNow.canComputeObservationsLikelihood(*sf);
1000 }
1001 
1002 /** Do not move the particles until the map is populated. */
1003 bool CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement() const
1004 {
1005  return 0 == getNumberOfObservationsInSimplemap();
1006 }
1007 
1008 /*---------------------------------------------------------------
1009  Evaluate the observation likelihood for one
1010  particle at a given location
1011  ---------------------------------------------------------------*/
1012 double CMultiMetricMapPDF::PF_SLAM_computeObservationLikelihoodForParticle(
1013  const CParticleFilter::TParticleFilterOptions& PF_options,
1014  const size_t particleIndexForMap, const CSensoryFrame& observation,
1015  const CPose3D& x) const
1016 {
1017  MRPT_UNUSED_PARAM(PF_options);
1018  auto* map = const_cast<CMultiMetricMap*>(
1019  &m_particles[particleIndexForMap].d->mapTillNow);
1020  double ret = 0;
1021  for (const auto& it : observation)
1022  ret += map->computeObservationLikelihood(*it, x);
1023  return ret;
1024 }
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
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
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: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019