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



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018