Main MRPT website > C++ reference for MRPT 1.9.9
CHMTSLAM_LSLAM_RBPF_2DLASER.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "hmtslam-precomp.h" // Precomp header
11 
14 #include <mrpt/math/wrap2pi.h>
15 #include <mrpt/slam/CICP.h>
16 
17 #include <mrpt/random.h>
18 #include <mrpt/math/utils.h>
20 #include <mrpt/system/CTicTac.h>
21 #include <mrpt/io/CFileStream.h>
23 #include <mrpt/system/os.h>
24 #include <mrpt/system/CTicTac.h>
25 
26 using namespace mrpt;
27 using namespace mrpt::slam;
28 using namespace mrpt::hmtslam;
29 using namespace mrpt::obs;
30 using namespace mrpt::random;
31 using namespace mrpt::system;
32 using namespace mrpt::maps;
33 using namespace mrpt::bayes;
34 using namespace mrpt::poses;
35 using namespace std;
36 
37 // Constructor
38 CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER(CHMTSLAM* parent)
39  : CLSLAMAlgorithmBase(parent)
40 {
41 }
42 // Destructor
44 /*---------------------------------------------------------------
45 
46  CLSLAM_RBPF_2DLASER
47 
48  Implements a 2D local SLAM method based on a RBPF
49  over an occupancy grid map. A part of HMT-SLAM.
50 
51 \param LMH The local metric hypothesis which must be updated by this SLAM
52 algorithm.
53 \param act The action to process (or nullptr).
54 \param sf The observations to process (or nullptr).
55 
56  WE ALREADY HAVE CONTROL OVER THE CRITICAL SECTION OF THE LMHs!
57 
58 --------------------------------------------------------------- */
61  const CSensoryFrame::Ptr& sf)
62 {
64 
65  // Get the current robot pose estimation:
66  TPoseID currentPoseID = LMH->m_currentRobotPose;
67 
68  // If this is the first iteration, just create a new robot pose at the
69  // origin:
70  if (currentPoseID == POSEID_INVALID)
71  {
72  currentPoseID = CHMTSLAM::generatePoseID();
73  LMH->m_currentRobotPose = currentPoseID; // Change it in the LMH
74 
75  // Create a new robot pose:
76  CPose3D initPose(0, 0, 0);
77 
78  ASSERT_(LMH->m_particles.size() > 0);
80  LMH->m_particles.begin();
81  it != LMH->m_particles.end(); ++it)
82  it->d->robotPoses[currentPoseID] = initPose;
83 
84  ASSERT_(m_parent->m_map.nodeCount() == 1);
85 
86  m_parent->m_map_cs.lock();
87  CHMHMapNode::Ptr firstArea = m_parent->m_map.getFirstNode();
88  ASSERT_(firstArea);
89  LMH->m_nodeIDmemberships[currentPoseID] = firstArea->getID();
90 
91  // Set annotation for the reference pose:
92  firstArea->m_annotations.setElemental(
93  NODE_ANNOTATION_REF_POSEID, currentPoseID, LMH->m_ID);
94  m_parent->m_map_cs.unlock();
95  }
96 
97  bool insertNewRobotPose = false;
98  if (sf)
99  {
100  if (LMH->m_nodeIDmemberships.size() < 2) // If there is just 1 node
101  // (the current robot pose),
102  // then there is no
103  // observation in the map yet!
104  { // Update map if this is the first observation!
105  insertNewRobotPose = true;
106  }
107  else
108  {
109  // Check minimum distance from current robot pose to those in the
110  // neighborhood:
111  TMapPoseID2Pose3D lstRobotPoses;
112  LMH->getMeans(lstRobotPoses);
113 
114  CPose3D* currentRobotPose = &lstRobotPoses[currentPoseID];
115  float minDistLin = 1e6f;
116  float minDistAng = 1e6f;
117 
118  // printf("Poses in graph:\n");
119  for (TMapPoseID2Pose3D::iterator it = lstRobotPoses.begin();
120  it != lstRobotPoses.end(); ++it)
121  {
122  if (it->first != currentPoseID)
123  {
124  float linDist = it->second.distanceTo(*currentRobotPose);
125  float angDist = fabs(
127  it->second.yaw() - currentRobotPose->yaw()));
128 
129  minDistLin = min(minDistLin, linDist);
130 
131  if (linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS)
132  minDistAng = min(minDistAng, angDist);
133  }
134  }
135 
136  // time to insert a new node??
137  insertNewRobotPose =
138  (minDistLin > m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) ||
139  (minDistAng > m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS);
140  }
141 
142  } // end if there are SF
143 
144  // Save data in members so PF callback "prediction_and_update_pfXXXX" have
145  // access to them:
146  m_insertNewRobotPose = insertNewRobotPose;
147 
148  // ------------------------------------------------
149  // Execute RBPF method:
150  // 1) PROCESS ACTION
151  // 2) PROCESS OBSERVATIONS
152  // ------------------------------------------------
153  CParticleFilter pf;
154  pf.m_options = m_parent->m_options.pf_options;
155  pf.executeOn(*LMH, actions.get(), sf.get());
156 
157  // 3) The appearance observation: update the log likelihood
158  // ...
159 
160  // -----------------------------------------------------------
161  // 4) UPDATE THE MAP
162  // -----------------------------------------------------------
163  if (insertNewRobotPose)
164  {
165  m_parent->logStr(
167  "[CLSLAM_RBPF_2DLASER] Adding new pose...\n");
168 
169  // Leave the up-to-now "current pose" in the map, insert the SF in it,
170  // and...
171  // ----------------------------------------------------------------------------
172  TPoseID newCurrentPoseID = CHMTSLAM::generatePoseID();
173 
174  // ...and create a new "current pose" making a copy of the previous
175  // one:
176  // and insert the observations into the metric maps:
177  // ----------------------------------------------------------------------------
179  LMH->m_particles.begin();
180  partIt != LMH->m_particles.end(); partIt++)
181  {
182  const CPose3D* curRobotPose = &partIt->d->robotPoses[currentPoseID];
183  partIt->d->robotPoses[newCurrentPoseID] = *curRobotPose;
184  sf->insertObservationsInto(&partIt->d->metricMaps, curRobotPose);
185  }
186 
187  // Add node membership: for now, a copy of the current pose:
188  LMH->m_nodeIDmemberships[newCurrentPoseID] =
189  LMH->m_nodeIDmemberships[currentPoseID];
190 
191  // Now, insert the SF in the just added robot pose:
192  // -----------------------------------------------------
193  LMH->m_SFs[currentPoseID] = *sf;
194 
195  // Sets the new poseID as current robot pose:
196  // ----------------------------------------------------
197  TPoseID newlyAddedPose = currentPoseID;
198  currentPoseID = LMH->m_currentRobotPose = newCurrentPoseID;
199 
200  // Mark the "newlyAddedPose" as pending to reconsider in the graph-cut
201  // method
202  // (Done in the main loop @ LSLAM thread)
203  // --------------------------------------------------------------------------------
204  LMH->m_posesPendingAddPartitioner.push_back(newlyAddedPose);
205 
206  m_parent->logFmt(
207  mrpt::system::LVL_INFO, "[CLSLAM_RBPF_2DLASER] Added pose %i.\n",
208  (int)newlyAddedPose);
209 
210  // Notice LC detectors:
211  // ------------------------------
212  {
213  std::lock_guard<std::mutex> lock(m_parent->m_topLCdets_cs);
214 
216  m_parent->m_topLCdets.begin();
217  it != m_parent->m_topLCdets.end(); ++it)
218  (*it)->OnNewPose(newlyAddedPose, sf.get());
219  }
220 
221  } // end of insertNewRobotPose
222 
223  MRPT_END
224 }
225 
226 /** The PF algorithm implementation for "optimal sampling for non-parametric
227  * observation models"
228  */
231  const mrpt::obs::CSensoryFrame* sf,
233 {
234  MRPT_START
235 
236  // Get the current robot pose estimation:
237  TPoseID currentPoseID = LMH->m_currentRobotPose;
238 
239  size_t i, k, N, M = LMH->m_particles.size();
240 
241  // ----------------------------------------------------------------------
242  // We can execute optimal PF only when we have both, an action, and
243  // a valid observation from which to compute the likelihood:
244  // Accumulate odometry/actions until we have a valid observation, then
245  // process them simultaneously.
246  // ----------------------------------------------------------------------
247  // static CActionRobotMovement2D accumRobotMovement;
248  // static bool accumRobotMovementIsValid = false;
249  bool SFhasValidObservations = false;
250  // A valid action?
251  if (actions != nullptr)
252  {
254  actions->getBestMovementEstimation(); // Find a robot movement
255  // estimation:
256  if (!act)
258  "Action list does not contain any CActionRobotMovement2D "
259  "derived object!");
260 
261  if (!LMH->m_accumRobotMovementIsValid) // Reset accum.
262  {
263  act->poseChange->getMean(
266  act->motionModelConfiguration;
267  }
268  else
271  act->poseChange->getMeanVal();
272 
273  LMH->m_accumRobotMovementIsValid = true;
274  }
275 
276  if (sf != nullptr)
277  {
278  ASSERT_(LMH->m_particles.size() > 0);
279  SFhasValidObservations =
280  (*LMH->m_particles.begin())
281  .d->metricMaps.canComputeObservationsLikelihood(*sf);
282  }
283 
284  // All the needed things?
285  if (!LMH->m_accumRobotMovementIsValid || !SFhasValidObservations)
286  return; // Nothing we can do here...
287 
288  // OK, we have all we need, let's start!
289 
290  // Take the accum. actions as input:
291  CActionRobotMovement2D theResultingRobotMov;
292 
293  // Over
294  keep_max(
296  .minStdXY,
297  LMH->m_parent->m_options.MIN_ODOMETRY_STD_XY);
298  keep_max(
300  .minStdPHI,
301  LMH->m_parent->m_options.MIN_ODOMETRY_STD_PHI);
302 
303  theResultingRobotMov.computeFromOdometry(
306 
307  const CActionRobotMovement2D* robotMovement = &theResultingRobotMov;
308 
310  false; // To reset odometry at next iteration!
311 
312  // ----------------------------------------------------------------------
313  // 0) Common part: Prepare m_particles "draw" and compute
314  // ----------------------------------------------------------------------
315  // Precompute a list of "random" samples from the movement model:
316  LMH->m_movementDraws.clear();
317 
318  // Fast pseudorandom generator of poses...
319  // m_movementDraws.resize(
320  // max(2000,(int)(PF_options.pfAuxFilterOptimal_MaximumSearchSamples *
321  // 5.6574) ) );
322  LMH->m_movementDraws.resize(
324  size_t size_movementDraws = LMH->m_movementDraws.size();
325  LMH->m_movementDrawsIdx = (unsigned int)floor(
327  0.0f, ((float)size_movementDraws) - 0.01f));
328 
329  robotMovement->prepareFastDrawSingleSamples();
330  for (size_t i = 0; i < LMH->m_movementDraws.size(); i++)
331  robotMovement->fastDrawSingleSample(LMH->m_movementDraws[i]);
332 
334  LMH->m_maxLikelihood.clear();
335  LMH->m_maxLikelihood.resize(M, 0);
336  LMH->m_movementDrawMaximumLikelihood.resize(M);
337 
338  // Prepare data for executing "fastDrawSample"
339  CTicTac tictac;
340  tictac.Tic();
342  PF_options, particlesEvaluator_AuxPFOptimal, robotMovement, sf);
343  printf("[prepareFastDrawSample] Done in %.06f ms\n", tictac.Tac() * 1e3f);
344 
345 #if 0
346  printf("[prepareFastDrawSample] max (log) = %10.06f\n", math::maximum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) );
347  printf("[prepareFastDrawSample] max-mean (log) = %10.06f\n", -math::mean(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) );
348  printf("[prepareFastDrawSample] max-min (log) = %10.06f\n", -math::minimum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) );
349 #endif
350 
351  // Now we have the vector "m_fastDrawProbability" filled out with:
352  // w[i]p(zt|z^{t-1},x^{[i],t-1},X)
353  // where X is the robot pose prior (as implemented in
354  // the aux. function "particlesEvaluator_AuxPFOptimal"),
355  // and also the "m_maxLikelihood" filled with the maximum lik. values.
357  vector<double> newParticlesWeight;
358  vector<size_t> newParticlesDerivedFromIdx;
359 
360  // We need the (aproximate) maximum likelihood value for each
361  // previous particle [i]:
362  //
363  // max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
364  //
365  // CVectorDouble maxLikelihood(M, -1 );
366 
367  float MIN_ACCEPT_UNIF_DISTRIB = 0.00f;
368 
369  CPose2D movementDraw, newPose, oldPose;
370  double acceptanceProb, newPoseLikelihood, ratioLikLik;
371  unsigned int statsTrialsCount = 0, statsTrialsSuccess = 0;
372  std::vector<bool> maxLikMovementDrawHasBeenUsed(M, false);
373  unsigned int statsWarningsAccProbAboveOne = 0;
374  // double maxMeanLik = math::maximum(
375  // LMH->m_pfAuxiliaryPFOptimal_estimatedProb ); // For normalization
376  // purposes only
377 
378  ASSERT_(!PF_options.adaptiveSampleSize);
379 
380  // ----------------------------------------------------------------------
381  // 1) FIXED SAMPLE SIZE VERSION
382  // ----------------------------------------------------------------------
383  newParticles.resize(M);
384  newParticlesWeight.resize(M);
385  newParticlesDerivedFromIdx.resize(M);
386 
387  bool doResample = LMH->ESS() < 0.5;
388 
389  for (i = 0; i < M; i++)
390  {
391  // Generate a new particle:
392  // (a) Draw a "t-1" m_particles' index:
393  // ----------------------------------------------------------------
394  if (doResample)
395  k = LMH->fastDrawSample(
396  PF_options); // Based on weights of last step only!
397  else
398  k = i;
399 
400  oldPose = CPose2D(*LMH->getCurrentPose(k));
401 
402  // (b) Rejection-sampling: Draw a new robot pose from x[k],
403  // and accept it with probability p(zk|x) / maxLikelihood:
404  // ----------------------------------------------------------------
405  if (LMH->m_SFs.empty())
406  {
407  // The first robot pose in the SLAM execution: All m_particles start
408  // at the same point (this is the lowest bound of subsequent
409  // uncertainty):
410  movementDraw = CPose2D(0, 0, 0);
411  newPose = oldPose; // + movementDraw;
412  }
413  else
414  {
415  // Rejection-sampling:
416  do
417  {
418  // Draw new robot pose:
419  if (!maxLikMovementDrawHasBeenUsed[k])
420  {
421  // No! first take advantage of a good drawn value, but only
422  // once!!
423  maxLikMovementDrawHasBeenUsed[k] = true;
424  movementDraw = LMH->m_movementDrawMaximumLikelihood[k];
425 #if 0
426  cout << "Drawn pose (max. lik): " << movementDraw << endl;
427 #endif
428  }
429  else
430  {
431  // Draw new robot pose:
432  // robotMovement->drawSingleSample( movementDraw );
433  // robotMovement->fastDrawSingleSample( movementDraw );
434  movementDraw =
435  LMH->m_movementDraws[LMH->m_movementDrawsIdx++ %
436  size_movementDraws];
437  }
438 
439  newPose.composeFrom(
440  oldPose,
441  movementDraw); // newPose = oldPose + movementDraw;
442 
443  // Compute acceptance probability:
444  newPoseLikelihood = auxiliarComputeObservationLikelihood(
445  PF_options, LMH, k, sf, &newPose);
446  ratioLikLik = exp(newPoseLikelihood - LMH->m_maxLikelihood[k]);
447  acceptanceProb = min(1.0, ratioLikLik);
448 
449  if (ratioLikLik > 1)
450  {
451  if (ratioLikLik > 1.5)
452  {
453  statsWarningsAccProbAboveOne++;
454  // DEBUG
455  // printf("[pfAuxiliaryPFOptimal] Warning!!
456  // p(z|x)/p(z|x*)=%f\n",ratioLikLik);
457  }
458  LMH->m_maxLikelihood[k] = newPoseLikelihood; // :'-( !!!
459  acceptanceProb = 0; // Keep searching!!
460  }
461 
462  statsTrialsCount++; // Stats
463 
464  } while (acceptanceProb < getRandomGenerator().drawUniform(
465  MIN_ACCEPT_UNIF_DISTRIB, 0.999f));
466 
467  statsTrialsSuccess++; // Stats:
468  }
469 
470  // Insert the new particle!:
471  newParticles[i] = newPose;
472 
473  // And its weight:
474  double weightFact =
475  LMH->m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.powFactor;
476 
477  // Add to historic record of log_w weights:
478  LMH->m_log_w_metric_history.resize(M);
479  LMH->m_log_w_metric_history[i][currentPoseID] += weightFact;
480 
481  if (doResample)
482  newParticlesWeight[i] = 0;
483  else
484  newParticlesWeight[i] = LMH->m_particles[k].log_w + weightFact;
485 
486  // and its heritance:
487  newParticlesDerivedFromIdx[i] = (unsigned int)k;
488 
489  } // for i
490 
491  // ---------------------------------------------------------------------------------
492  // Substitute old by new particle set:
493  // Old are in "m_particles"
494  // New are in "newParticles",
495  // "newParticlesWeight","newParticlesDerivedFromIdx"
496  // ---------------------------------------------------------------------------------
497  N = newParticles.size();
498  CLocalMetricHypothesis::CParticleList newParticlesArray(N);
500 
501  // For efficiency, just copy the "CRBPFParticleData" from the old particle
502  // into the
503  // new one, but this can be done only once:
504  std::vector<bool> oldParticleAlreadyCopied(LMH->m_particles.size(), false);
505  CLSLAMParticleData* newPartData;
506 
507  for (newPartIt = newParticlesArray.begin(), i = 0;
508  newPartIt != newParticlesArray.end(); newPartIt++, i++)
509  {
510  // The weight:
511  newPartIt->log_w = newParticlesWeight[i];
512 
513  // The data (CRBPFParticleData):
514  if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
515  {
516  // The first copy of this old particle:
517  newPartData =
518  LMH->m_particles[newParticlesDerivedFromIdx[i]].d.release();
519  oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] = true;
520  }
521  else
522  {
523  // Make a copy:
524  newPartData = new CLSLAMParticleData(
525  *LMH->m_particles[newParticlesDerivedFromIdx[i]].d);
526  }
527 
528  newPartIt->d.reset(newPartData);
529  } // end for "newPartIt"
530 
531  // Now add the new robot pose to the paths: (this MUST be done after the
532  // above loop, separately):
533  for (newPartIt = newParticlesArray.begin(), i = 0;
534  newPartIt != newParticlesArray.end(); newPartIt++, i++)
535  newPartIt->d->robotPoses[LMH->m_currentRobotPose] =
536  CPose3D(newParticles[i]);
537 
538  // Free those old m_particles not being copied into the new ones:
539  for (i = 0; i < LMH->m_particles.size(); i++)
540  {
541  LMH->m_particles[i].d.reset();
542  }
543 
544  // Copy into "m_particles":
545  LMH->m_particles.resize(newParticlesArray.size());
546  for (newPartIt = newParticlesArray.begin(),
547  trgPartIt = LMH->m_particles.begin();
548  newPartIt != newParticlesArray.end(); newPartIt++, trgPartIt++)
549  {
550  trgPartIt->log_w = newPartIt->log_w;
551  trgPartIt->d.move_from(newPartIt->d);
552  }
553 
554  // Free buffers:
555  newParticles.clear();
556  newParticlesArray.clear();
557  newParticlesWeight.clear();
558  newParticlesDerivedFromIdx.clear();
559 
560  double out_max_log_w;
561  LMH->normalizeWeights(&out_max_log_w); // Normalize weights:
562  LMH->m_log_w += out_max_log_w;
563 
564 #if 0
565  printf("[REJ-SAMP.RATIO: \t%.03f%% \t %u out of %u with P(acep)>1]\n\n",
566  100.0f*statsTrialsSuccess / (float)(max(1u,statsTrialsCount)),
567  statsWarningsAccProbAboveOne,
568  statsTrialsCount
569  );
570 #endif
571 
572  MRPT_END
573 }
574 
575 /*---------------------------------------------------------------
576  particlesEvaluator_AuxPFOptimal
577  ---------------------------------------------------------------*/
580  const CParticleFilterCapable* obj, size_t index, const void* action,
581  const void* observation)
582 {
583  MRPT_UNUSED_PARAM(action);
584 
585  MRPT_START
586 
587  const CLocalMetricHypothesis* myObj =
588  static_cast<const CLocalMetricHypothesis*>(obj);
589 
590  // Compute the quantity:
591  // w[i]p(zt|z^{t-1},x^{[i],t-1})
592  // As the Monte-Carlo approximation of the
593  // integral over all posible $x_t$.
594 
595  // Take the previous particle weight:
596  // --------------------------------------------
597  double indivLik, maxLik = -std::numeric_limits<double>::max();
598  size_t maxLikDraw = 0;
599  size_t N;
600  size_t nDraws = myObj->m_movementDraws.size();
601 
602  ASSERT_(nDraws > 1);
603 
604  // , perform a Monte-Carlo approximation:
605  // --------------------------------------------
607  ASSERT_(N > 1);
608 
609  CPose2D oldPose(*myObj->getCurrentPose(index));
610  // CPose2D drawnSample;
611  mrpt::math::CVectorDouble vectLiks(
612  N, 0); // The vector with the individual log-likelihoods.
613 
614  for (size_t q = 0; q < N; q++)
615  {
616  CPose2D x_predict(
617  oldPose +
618  myObj->m_movementDraws[(++myObj->m_movementDrawsIdx) % nDraws]);
619 
620  // Estimate the mean...
622  PF_options, obj, index, ((CSensoryFrame*)observation), &x_predict);
623 
624  MRPT_CHECK_NORMAL_NUMBER(indivLik);
625  vectLiks[q] = indivLik;
626 
627  // and the maximum value:
628  if (indivLik > maxLik)
629  {
630  maxLikDraw = myObj->m_movementDrawsIdx % nDraws;
631  maxLik = indivLik;
632  }
633  }
634 
635  // This is done to avoid floating point overflow!!
636  double maxLogLik = math::maximum(vectLiks);
637  vectLiks.array() -=
638  maxLogLik; // Maximum log-lik = 0 (max linear likelihood=1)
639 
640  // average_lik = \sum(e^liks) * e^maxLik / N
641  // log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )
642  double avrgLogLik =
643  log(vectLiks.array().exp().sum()) + maxLogLik - log((double)N);
644 
645  // Save into the object:
646  myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index] =
647  avrgLogLik; // log( accum / N );
648  myObj->m_maxLikelihood[index] = maxLik;
649  myObj->m_movementDrawMaximumLikelihood[index] =
650  myObj->m_movementDraws[maxLikDraw];
651 
652  // and compute the resulting probability of this particle:
653  // ------------------------------------------------------------
654  // if (myObj->m_adaptiveSampleSize)
655  // return myObj->m_particles[index].w *
656  // myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
657  // else return myObj->m_particles[index].w;
658 
659  double ret = myObj->m_particles[index].log_w +
660  myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
661 
663 
664  return ret;
665 
666  MRPT_END
667 }
668 
669 /*---------------------------------------------------------------
670  auxiliarComputeObservationLikelihood
671  ---------------------------------------------------------------*/
674  const CParticleFilterCapable* obj, size_t particleIndexForMap,
675  const CSensoryFrame* observation, const CPose2D* x)
676 {
677  MRPT_UNUSED_PARAM(PF_options);
678  const CLocalMetricHypothesis* theObj =
679  static_cast<const CLocalMetricHypothesis*>(obj);
680  CMultiMetricMap* map = const_cast<CMultiMetricMap*>(
681  &theObj->m_particles[particleIndexForMap].d->metricMaps);
682 
683  return map->computeObservationsLikelihood(*observation, *x);
684 }
685 
686 /*---------------------------------------------------------------
687  dumpToStdOut
688  ---------------------------------------------------------------*/
690 {
692 
693  std::cout << "x = [";
694  for (it = x.begin(); it != x.end(); it++) std::cout << *it << " ";
695  std::cout << "]" << std::endl;
696 
697  std::cout << "y = [";
698  for (it = y.begin(); it != y.end(); it++) std::cout << *it << " ";
699  std::cout << "]" << std::endl;
700 
701  std::cout << "Phi = [";
702  for (it = phi.begin(); it != phi.end(); it++) std::cout << *it << " ";
703  std::cout << "]" << std::endl;
704 }
705 
706 /*---------------------------------------------------------------
707  loadTPathBinFromPath
708  ---------------------------------------------------------------*/
711  CPose2D* newPose)
712 {
713  size_t lenBinPath;
714 
715  if (path != nullptr)
716  lenBinPath = path->size();
717  else
718  lenBinPath = 0;
719 
721  std::vector<int>::iterator itX, itY, itPHI;
722 
723  // Set the output bin dimensionality:
724  outBin.x.resize(lenBinPath + (newPose != nullptr ? 1 : 0));
725  outBin.y.resize(lenBinPath + (newPose != nullptr ? 1 : 0));
726  outBin.phi.resize(lenBinPath + (newPose != nullptr ? 1 : 0));
727 
728  // Is a path provided??
729  if (path != nullptr)
730  {
731  // Fill the bin data:
732  for (itSrc = path->begin(), itX = outBin.x.begin(),
733  itY = outBin.y.begin(), itPHI = outBin.phi.begin();
734  itSrc != path->end(); itSrc++, itX++, itY++, itPHI++)
735  {
736  *itX = (int)round(
737  itSrc->second.x() /
738  m_parent->m_options.KLD_params.KLD_binSize_XY);
739  *itY = (int)round(
740  itSrc->second.y() /
741  m_parent->m_options.KLD_params.KLD_binSize_XY);
742  *itPHI = (int)round(
743  itSrc->second.yaw() /
744  m_parent->m_options.KLD_params.KLD_binSize_PHI);
745  } // end-for build path bin
746  }
747 
748  // Is a newPose provided??
749  if (newPose != nullptr)
750  {
751  // And append the last pose: the new one:
752  outBin.x[lenBinPath] = (int)round(
753  newPose->x() / m_parent->m_options.KLD_params.KLD_binSize_XY);
754  outBin.y[lenBinPath] = (int)round(
755  newPose->y() / m_parent->m_options.KLD_params.KLD_binSize_XY);
756  outBin.phi[lenBinPath] = (int)round(
757  newPose->phi() / m_parent->m_options.KLD_params.KLD_binSize_PHI);
758  }
759 }
760 
761 /*---------------------------------------------------------------
762  findTPathBinIntoSet
763  ---------------------------------------------------------------*/
765  CLSLAM_RBPF_2DLASER::TPathBin& desiredBin,
766  std::deque<CLSLAM_RBPF_2DLASER::TPathBin>& theSet)
767 {
768  // it = pathBins.find( p ); <---- This doesn't work!!!
769  // TODO: A more efficient search algorithm here!!
771  int ret;
772 
773  for (it = theSet.begin(), ret = 0; it != theSet.end(); it++, ret++)
774  if ((it->x == desiredBin.x) && (it->y == desiredBin.y) &&
775  (it->phi == desiredBin.phi))
776  return ret;
777 
778  // Not found!
779  return -1;
780 }
781 
782 /** The PF algorithm implementation for "optimal sampling" approximated with
783  * scan matching (Stachniss method)
784  */
787  const mrpt::obs::CSensoryFrame* sf,
789 {
790  MRPT_START
791 
792  CTicTac tictac;
793 
794  // Get the current robot pose estimation:
795  TPoseID currentPoseID = LMH->m_currentRobotPose;
796 
797  // ----------------------------------------------------------------------
798  // We can execute optimal PF only when we have both, an action, and
799  // a valid observation from which to compute the likelihood:
800  // Accumulate odometry/actions until we have a valid observation, then
801  // process them simultaneously.
802  // ----------------------------------------------------------------------
803  bool SFhasValidObservations = false;
804  // A valid action?
805  if (actions != nullptr)
806  {
808  actions->getBestMovementEstimation(); // Find a robot movement
809  // estimation:
810  if (!act)
812  "Action list does not contain any CActionRobotMovement2D "
813  "derived object!");
814 
815  if (!LMH->m_accumRobotMovementIsValid) // Reset accum.
816  {
817  act->poseChange->getMean(
820  act->motionModelConfiguration;
821  }
822  else
825  act->poseChange->getMeanVal();
826 
827  LMH->m_accumRobotMovementIsValid = true;
828  }
829 
830  if (sf != nullptr)
831  {
832  ASSERT_(LMH->m_particles.size() > 0);
833  SFhasValidObservations =
834  (*LMH->m_particles.begin())
835  .d->metricMaps.canComputeObservationsLikelihood(*sf);
836  }
837 
838  // All the needed things?
839  if (!LMH->m_accumRobotMovementIsValid || !SFhasValidObservations)
840  return; // Nothing we can do here...
841  ASSERT_(sf != nullptr);
842  ASSERT_(!PF_options.adaptiveSampleSize);
843 
844  // OK, we have all we need, let's start!
845 
846  // The odometry-based increment since last step is
847  // in: LMH->m_accumRobotMovement.rawOdometryIncrementReading
848  const CPose2D initialPoseEstimation =
851  false; // To reset odometry at next iteration!
852 
853  // ----------------------------------------------------------------------
854  // 1) FIXED SAMPLE SIZE VERSION
855  // ----------------------------------------------------------------------
856 
857  // ICP used if "pfOptimalProposal_mapSelection" = 0 or 1
858  CICP icp;
859  CICP::TReturnInfo icpInfo;
860 
861  // ICP options
862  // ------------------------------
863  icp.options.maxIterations = 80;
864  icp.options.thresholdDist = 0.50f;
865  icp.options.thresholdAng = DEG2RAD(20);
866  icp.options.smallestThresholdDist = 0.05f;
867  icp.options.ALFA = 0.5f;
868  icp.options.doRANSAC = false;
869 
870  // Build the map of points to align:
871  CSimplePointsMap localMapPoints;
872 
873  ASSERT_(LMH->m_particles[0].d->metricMaps.m_gridMaps.size() > 0);
874  // float minDistBetweenPointsInLocalMaps = 0.02f; //3.0f *
875  // m_particles[0].d->metricMaps.m_gridMaps[0]->getResolution();
876 
877  // Build local map:
878  localMapPoints.clear();
879  localMapPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f;
880  sf->insertObservationsInto(&localMapPoints);
881 
882  // Process the particles
883  const size_t M = LMH->m_particles.size();
884  LMH->m_log_w_metric_history.resize(M);
885 
886  for (size_t i = 0; i < M; i++)
887  {
889  CPose3D* part_pose = LMH->getCurrentPose(i);
890 
891  if (LMH->m_SFs.empty())
892  {
893  // The first robot pose in the SLAM execution: All m_particles start
894  // at the same point (this is the lowest bound of subsequent
895  // uncertainty):
896  // New pose = old pose.
897  // part_pose: Unmodified
898  }
899  else
900  {
901  // ICP and add noise:
902  CPosePDFGaussian icpEstimation;
903 
904  // Select map to use with ICP:
905  CMetricMap* mapalign;
906 
907  if (!part.d->metricMaps.m_pointsMaps.empty())
908  mapalign = part.d->metricMaps.m_pointsMaps[0].get();
909  else if (!part.d->metricMaps.m_gridMaps.empty())
910  mapalign = part.d->metricMaps.m_gridMaps[0].get();
911  else
913  "There is no point or grid map. At least one needed for "
914  "ICP.");
915 
916  // Use ICP to align to each particle's map:
917  CPosePDF::Ptr alignEst = icp.Align(
918  mapalign, &localMapPoints, initialPoseEstimation, nullptr,
919  &icpInfo);
920 
921  icpEstimation.copyFrom(*alignEst);
922 
923 #if 0
924  // HACK:
925  CPose3D Ap = finalEstimatedPoseGauss.mean - ith_last_pose;
926  double Ap_dist = Ap.norm();
927  finalEstimatedPoseGauss.cov.zeros();
928  finalEstimatedPoseGauss.cov(0,0) = square( fabs(Ap_dist)*0.01 );
929  finalEstimatedPoseGauss.cov(1,1) = square( fabs(Ap_dist)*0.01 );
930  finalEstimatedPoseGauss.cov(2,2) = square( fabs(Ap.yaw())*0.02 );
931 #endif
932 
933  // Generate gaussian-distributed 2D-pose increments according to
934  // "finalEstimatedPoseGauss":
935  // -------------------------------------------------------------------------------------------
936  // Set the gaussian pose:
937  CPose3DPDFGaussian finalEstimatedPoseGauss(icpEstimation);
938 
939  CPose3D noisy_increment;
940  finalEstimatedPoseGauss.drawSingleSample(noisy_increment);
941 
942  CPose3D new_pose;
943  new_pose.composeFrom(*part_pose, noisy_increment);
944 
945  CPose2D new_pose2d = CPose2D(new_pose);
946 
947  // Add the pose to the path:
948  part.d->robotPoses[LMH->m_currentRobotPose] = new_pose;
949 
950  // Update the weight:
951  // ---------------------------------------------------------------------------
952  const double log_lik =
954  PF_options, LMH, i, sf, &new_pose2d);
955 
956  part.log_w += log_lik;
957 
958  // Add to historic record of log_w weights:
959  LMH->m_log_w_metric_history[i][currentPoseID] += log_lik;
960 
961  } // end else we can do ICP
962 
963  } // end for each particle i
964 
965  // Accumulate the log likelihood of this LMH as a whole:
966  double out_max_log_w;
967  LMH->normalizeWeights(&out_max_log_w); // Normalize weights:
968  LMH->m_log_w += out_max_log_w;
969 
970  printf("[CLSLAM_RBPF_2DLASER] Overall likelihood = %.2e\n", out_max_log_w);
971  printf("[CLSLAM_RBPF_2DLASER] Done in %.03fms\n", 1e3 * tictac.Tac());
972 
973  MRPT_END
974 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:31
int findTPathBinIntoSet(TPathBin &desiredBin, std::deque< TPathBin > &theSet)
Checks if a given "TPathBin" element is already into a set of them, and return its index (first one i...
A namespace of pseudo-random numbers generators of diferent distributions.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:90
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:140
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Scalar * iterator
Definition: eigen_plugins.h:26
#define MRPT_START
Definition: exceptions.h:262
#define min(a, b)
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:211
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
CParticleList m_particles
The array of particles.
double normalizeWeights(double *out_max_log_w=nullptr) override
Normalize the (logarithmic) weights, such as the maximum weight is zero.
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
static TPoseID generatePoseID()
Generates a new and unique pose ID.
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.
mrpt::aligned_std_vector< mrpt::poses::CPose2D > m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:67
double DEG2RAD(const double x)
Degrees to radians.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
std::vector< double > m_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
This file implements several operations that operate element-wise on individual or pairs of container...
void prepareFastDrawSample(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, TParticleProbabilityEvaluator partEvaluator=defaultEvaluator, const void *action=nullptr, const void *observation=nullptr) const
Prepares data structures for calling fastDrawSample method next.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
A high-performance stopwatch, with typical resolution of nanoseconds.
void processOneLMH(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection::Ptr &act, const mrpt::obs::CSensoryFrame::Ptr &sf)
Main entry point from HMT-SLAM: process some actions & observations.
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:528
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
double m_log_w
Log-weight of this hypothesis.
void computeFromOdometry(const mrpt::poses::CPose2D &odometryIncrement, const TMotionModelOptions &options)
Computes the PDF of the pose increment from an odometry reading and according to the given motion mod...
STL namespace.
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
Definition: CHMTSLAM.h:517
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
Definition: CICP.h:122
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:182
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:68
static double particlesEvaluator_AuxPFOptimal(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".
Declares a class for storing a collection of robot actions.
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
T square(const T x)
Inline function for the square of a number.
CONTAINER::Scalar minimum(const CONTAINER &v)
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void loadTPathBinFromPath(TPathBin &outBin, TMapPoseID2Pose3D *path=nullptr, mrpt::poses::CPose2D *newPose=nullptr)
Fills out a "TPathBin" variable, given a path hypotesis and (if not set to nullptr) a new pose append...
Represents a probabilistic 2D movement of the robot mobile base.
std::vector< std::map< TPoseID, double > > m_log_w_metric_history
The historic log-weights of the metric observations inserted in this LMH, for each particle...
mrpt::aligned_std_map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
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...
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
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...
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
Definition: exceptions.h:118
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
Definition: CPose3D.cpp:565
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
double norm() const
Returns the euclidean norm of vector: .
Definition: CPoseOrPoint.h:253
#define POSEID_INVALID
GLuint index
Definition: glext.h:4054
mrpt::safe_ptr< CHMTSLAM > m_parent
Definition: CHMTSLAM.h:522
double computeObservationsLikelihood(const mrpt::obs::CSensoryFrame &sf, const mrpt::poses::CPose2D &takenFrom)
Returns the sum of the log-likelihoods of each individual observation within a mrpt::obs::CSensoryFra...
Definition: CMetricMap.cpp:67
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:103
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
This virtual class defines the interface that any particles based PDF class must implement in order t...
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
unsigned int m_movementDrawsIdx
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
Definition: CICP.h:135
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
CONTAINER::Scalar maximum(const CONTAINER &v)
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel gaussianModel
mrpt::aligned_std_vector< mrpt::poses::CPose2D > m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void prepareFastDrawSingleSamples() const
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "dr...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=nullptr)
Executes a complete prediction + update step of the selected particle filtering algorithm.
bool m_accumRobotMovementIsValid
Used in CLSLAM_RBPF_2DLASER.
A template class for holding a the data and the weight of a particle.
void getMeans(TMapPoseID2Pose3D &outList) const
Returns the mean of each robot pose in this LMH, as computed from the set of particles.
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
double ALFA
The scale factor for thresholds everytime convergence is achieved.
Definition: CICP.h:119
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:82
The configuration of a particle filter.
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:111
#define MRPT_END
Definition: exceptions.h:266
The ICP algorithm return information.
Definition: CICP.h:192
static double auxiliarComputeObservationLikelihood(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t particleIndexForMap, const mrpt::obs::CSensoryFrame *observation, const mrpt::poses::CPose2D *x)
Auxiliary function that evaluates the likelihood of an observation, given a robot pose...
double mean(const CONTAINER &v)
Computes the mean value of a vector.
#define NODE_ANNOTATION_REF_POSEID
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
GLenum GLint GLint y
Definition: glext.h:3538
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:257
GLenum GLint x
Definition: glext.h:3538
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:79
This class stores any customizable set of metric maps.
bool m_insertNewRobotPose
For use within PF callback methods.
Definition: CHMTSLAM.h:611
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter...
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).
double thresholdDist
Initial threshold distance for two points to become a correspondence.
Definition: CICP.h:116
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motionModelConfiguration
void prediction_and_update_pfAuxiliaryPFOptimal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
const Scalar * const_iterator
Definition: eigen_plugins.h:27
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
const mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx) const
Returns the i&#39;th particle hypothesis for the current robot pose.
size_t fastDrawSample(const bayes::CParticleFilter::TParticleFilterOptions &PF_options) const
Draws a random sample from the particle filter, in such a way that each particle has a probability pr...
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
mrpt::poses::CPose2D rawOdometryIncrementReading
This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emO...
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
Definition: CPointsMap.h:218
void prediction_and_update_pfOptimalProposal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
#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: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019