MRPT  1.9.9
CHMTSLAM_LSLAM_RBPF_2DLASER.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 "hmtslam-precomp.h" // Precomp header
11 
14 #include <mrpt/io/CFileStream.h>
16 #include <mrpt/math/utils.h>
17 #include <mrpt/math/wrap2pi.h>
19 #include <mrpt/random.h>
20 #include <mrpt/slam/CICP.h>
21 #include <mrpt/system/CTicTac.h>
22 #include <mrpt/system/os.h>
23 #include <Eigen/Dense>
24 
25 using namespace mrpt;
26 using namespace mrpt::slam;
27 using namespace mrpt::hmtslam;
28 using namespace mrpt::obs;
29 using namespace mrpt::random;
30 using namespace mrpt::system;
31 using namespace mrpt::maps;
32 using namespace mrpt::bayes;
33 using namespace mrpt::poses;
34 using namespace std;
35 
36 // Constructor
37 CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER(CHMTSLAM* parent)
38  : CLSLAMAlgorithmBase(parent)
39 {
40 }
41 // Destructor
43 /*---------------------------------------------------------------
44 
45  CLSLAM_RBPF_2DLASER
46 
47  Implements a 2D local SLAM method based on a RBPF
48  over an occupancy grid map. A part of HMT-SLAM.
49 
50 \param LMH The local metric hypothesis which must be updated by this SLAM
51 algorithm.
52 \param act The action to process (or nullptr).
53 \param sf The observations to process (or nullptr).
54 
55  WE ALREADY HAVE CONTROL OVER THE CRITICAL SECTION OF THE LMHs!
56 
57 --------------------------------------------------------------- */
60  const CSensoryFrame::Ptr& sf)
61 {
63 
64  // Get the current robot pose estimation:
65  TPoseID currentPoseID = LMH->m_currentRobotPose;
66 
67  // If this is the first iteration, just create a new robot pose at the
68  // origin:
69  if (currentPoseID == POSEID_INVALID)
70  {
71  currentPoseID = CHMTSLAM::generatePoseID();
72  LMH->m_currentRobotPose = currentPoseID; // Change it in the LMH
73 
74  // Create a new robot pose:
75  CPose3D initPose(0, 0, 0);
76 
77  ASSERT_(LMH->m_particles.size() > 0);
78  for (auto& m_particle : LMH->m_particles)
79  m_particle.d->robotPoses[currentPoseID] = initPose;
80 
81  ASSERT_(m_parent->m_map.nodeCount() == 1);
82 
83  m_parent->m_map_cs.lock();
84  CHMHMapNode::Ptr firstArea = m_parent->m_map.getFirstNode();
85  ASSERT_(firstArea);
86  LMH->m_nodeIDmemberships[currentPoseID] = firstArea->getID();
87 
88  // Set annotation for the reference pose:
89  firstArea->m_annotations.setElemental(
90  NODE_ANNOTATION_REF_POSEID, currentPoseID, LMH->m_ID);
91  m_parent->m_map_cs.unlock();
92  }
93 
94  bool insertNewRobotPose = false;
95  if (sf)
96  {
97  if (LMH->m_nodeIDmemberships.size() < 2) // If there is just 1 node
98  // (the current robot pose),
99  // then there is no
100  // 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
107  // neighborhood:
108  TMapPoseID2Pose3D lstRobotPoses;
109  LMH->getMeans(lstRobotPoses);
110 
111  CPose3D* currentRobotPose = &lstRobotPoses[currentPoseID];
112  float minDistLin = 1e6f;
113  float minDistAng = 1e6f;
114 
115  // printf("Poses in graph:\n");
116  for (auto& lstRobotPose : lstRobotPoses)
117  {
118  if (lstRobotPose.first != currentPoseID)
119  {
120  float linDist =
121  lstRobotPose.second.distanceTo(*currentRobotPose);
122  float angDist = fabs(math::wrapToPi(
123  lstRobotPose.second.yaw() - currentRobotPose->yaw()));
124 
125  minDistLin = min(minDistLin, linDist);
126 
127  if (linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS)
128  minDistAng = min(minDistAng, angDist);
129  }
130  }
131 
132  // time to insert a new node??
133  insertNewRobotPose =
134  (minDistLin > m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) ||
135  (minDistAng > m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS);
136  }
137 
138  } // end if there are SF
139 
140  // Save data in members so PF callback "prediction_and_update_pfXXXX" have
141  // access to them:
142  m_insertNewRobotPose = insertNewRobotPose;
143 
144  // ------------------------------------------------
145  // Execute RBPF method:
146  // 1) PROCESS ACTION
147  // 2) PROCESS OBSERVATIONS
148  // ------------------------------------------------
149  CParticleFilter pf;
150  pf.m_options = m_parent->m_options.pf_options;
151  pf.executeOn(*LMH, actions.get(), sf.get());
152 
153  // 3) The appearance observation: update the log likelihood
154  // ...
155 
156  // -----------------------------------------------------------
157  // 4) UPDATE THE MAP
158  // -----------------------------------------------------------
159  if (insertNewRobotPose)
160  {
161  m_parent->logStr(
163  "[CLSLAM_RBPF_2DLASER] Adding new pose...\n");
164 
165  // Leave the up-to-now "current pose" in the map, insert the SF in it,
166  // and...
167  // ----------------------------------------------------------------------------
168  TPoseID newCurrentPoseID = CHMTSLAM::generatePoseID();
169 
170  // ...and create a new "current pose" making a copy of the previous
171  // one:
172  // and insert the observations into the metric maps:
173  // ----------------------------------------------------------------------------
174  for (auto& m_particle : LMH->m_particles)
175  {
176  const CPose3D* curRobotPose =
177  &m_particle.d->robotPoses[currentPoseID];
178  m_particle.d->robotPoses[newCurrentPoseID] = *curRobotPose;
179  sf->insertObservationsInto(&m_particle.d->metricMaps, curRobotPose);
180  }
181 
182  // Add node membership: for now, a copy of the current pose:
183  LMH->m_nodeIDmemberships[newCurrentPoseID] =
184  LMH->m_nodeIDmemberships[currentPoseID];
185 
186  // Now, insert the SF in the just added robot pose:
187  // -----------------------------------------------------
188  LMH->m_SFs[currentPoseID] = *sf;
189 
190  // Sets the new poseID as current robot pose:
191  // ----------------------------------------------------
192  TPoseID newlyAddedPose = currentPoseID;
193  currentPoseID = LMH->m_currentRobotPose = newCurrentPoseID;
194 
195  // Mark the "newlyAddedPose" as pending to reconsider in the graph-cut
196  // method
197  // (Done in the main loop @ LSLAM thread)
198  // --------------------------------------------------------------------------------
199  LMH->m_posesPendingAddPartitioner.push_back(newlyAddedPose);
200 
201  m_parent->logFmt(
202  mrpt::system::LVL_INFO, "[CLSLAM_RBPF_2DLASER] Added pose %i.\n",
203  (int)newlyAddedPose);
204 
205  // Notice LC detectors:
206  // ------------------------------
207  {
208  std::lock_guard<std::mutex> lock(m_parent->m_topLCdets_cs);
209 
210  for (auto& m_topLCdet : m_parent->m_topLCdets)
211  m_topLCdet->OnNewPose(newlyAddedPose, sf.get());
212  }
213 
214  } // end of insertNewRobotPose
215 
216  MRPT_END
217 }
218 
219 /** The PF algorithm implementation for "optimal sampling for non-parametric
220  * observation models"
221  */
224  const mrpt::obs::CSensoryFrame* sf,
226 {
227  MRPT_START
228 
229  // Get the current robot pose estimation:
230  TPoseID currentPoseID = LMH->m_currentRobotPose;
231 
232  size_t i, k, N, M = LMH->m_particles.size();
233 
234  // ----------------------------------------------------------------------
235  // We can execute optimal PF only when we have both, an action, and
236  // a valid observation from which to compute the likelihood:
237  // Accumulate odometry/actions until we have a valid observation, then
238  // process them simultaneously.
239  // ----------------------------------------------------------------------
240  // static CActionRobotMovement2D accumRobotMovement;
241  // static bool accumRobotMovementIsValid = false;
242  bool SFhasValidObservations = false;
243  // A valid action?
244  if (actions != nullptr)
245  {
247  actions->getBestMovementEstimation(); // Find a robot movement
248  // estimation:
249  if (!act)
251  "Action list does not contain any CActionRobotMovement2D "
252  "derived object!");
253 
254  if (!LMH->m_accumRobotMovementIsValid) // Reset accum.
255  {
256  act->poseChange->getMean(
259  act->motionModelConfiguration;
260  }
261  else
264  act->poseChange->getMeanVal();
265 
266  LMH->m_accumRobotMovementIsValid = true;
267  }
268 
269  if (sf != nullptr)
270  {
271  ASSERT_(LMH->m_particles.size() > 0);
272  SFhasValidObservations =
273  (*LMH->m_particles.begin())
274  .d->metricMaps.canComputeObservationsLikelihood(*sf);
275  }
276 
277  // All the needed things?
278  if (!LMH->m_accumRobotMovementIsValid || !SFhasValidObservations)
279  return; // Nothing we can do here...
280 
281  // OK, we have all we need, let's start!
282 
283  // Take the accum. actions as input:
284  CActionRobotMovement2D theResultingRobotMov;
285 
286  // Over
287  keep_max(
289  .minStdXY,
290  LMH->m_parent->m_options.MIN_ODOMETRY_STD_XY);
291  keep_max(
293  .minStdPHI,
294  LMH->m_parent->m_options.MIN_ODOMETRY_STD_PHI);
295 
296  theResultingRobotMov.computeFromOdometry(
299 
300  const CActionRobotMovement2D* robotMovement = &theResultingRobotMov;
301 
303  false; // To reset odometry at next iteration!
304 
305  // ----------------------------------------------------------------------
306  // 0) Common part: Prepare m_particles "draw" and compute
307  // ----------------------------------------------------------------------
308  // Precompute a list of "random" samples from the movement model:
309  LMH->m_movementDraws.clear();
310 
311  // Fast pseudorandom generator of poses...
312  // m_movementDraws.resize(
313  // max(2000,(int)(PF_options.pfAuxFilterOptimal_MaximumSearchSamples *
314  // 5.6574) ) );
315  LMH->m_movementDraws.resize(
317  size_t size_movementDraws = LMH->m_movementDraws.size();
318  LMH->m_movementDrawsIdx =
319  (unsigned int)floor(getRandomGenerator().drawUniform(
320  0.0f, ((float)size_movementDraws) - 0.01f));
321 
322  robotMovement->prepareFastDrawSingleSamples();
323  for (auto& m_movementDraw : LMH->m_movementDraws)
324  robotMovement->fastDrawSingleSample(m_movementDraw);
325 
327  LMH->m_maxLikelihood.clear();
328  LMH->m_maxLikelihood.resize(M, 0);
329  LMH->m_movementDrawMaximumLikelihood.resize(M);
330 
331  // Prepare data for executing "fastDrawSample"
332  CTicTac tictac;
333  tictac.Tic();
335  PF_options, particlesEvaluator_AuxPFOptimal, robotMovement, sf);
336  printf("[prepareFastDrawSample] Done in %.06f ms\n", tictac.Tac() * 1e3f);
337 
338 #if 0
339  printf("[prepareFastDrawSample] max (log) = %10.06f\n", math::maximum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) );
340  printf("[prepareFastDrawSample] max-mean (log) = %10.06f\n", -math::mean(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) );
341  printf("[prepareFastDrawSample] max-min (log) = %10.06f\n", -math::minimum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) + math::maximum(LMH->m_pfAuxiliaryPFOptimal_estimatedProb) );
342 #endif
343 
344  // Now we have the vector "m_fastDrawProbability" filled out with:
345  // w[i]p(zt|z^{t-1},x^{[i],t-1},X)
346  // where X is the robot pose prior (as implemented in
347  // the aux. function "particlesEvaluator_AuxPFOptimal"),
348  // and also the "m_maxLikelihood" filled with the maximum lik. values.
349  std::vector<mrpt::poses::CPose2D> newParticles;
350  vector<double> newParticlesWeight;
351  vector<size_t> newParticlesDerivedFromIdx;
352 
353  // We need the (aproximate) maximum likelihood value for each
354  // previous particle [i]:
355  //
356  // max{ p( z^t | data^[i], x_(t-1)^[i], u_(t) ) }
357  //
358  // CVectorDouble maxLikelihood(M, -1 );
359 
360  float MIN_ACCEPT_UNIF_DISTRIB = 0.00f;
361 
362  CPose2D movementDraw, newPose, oldPose;
363  double acceptanceProb, newPoseLikelihood, ratioLikLik;
364  unsigned int statsTrialsCount = 0, statsTrialsSuccess = 0;
365  std::vector<bool> maxLikMovementDrawHasBeenUsed(M, false);
366  unsigned int statsWarningsAccProbAboveOne = 0;
367  // double maxMeanLik = math::maximum(
368  // LMH->m_pfAuxiliaryPFOptimal_estimatedProb ); // For normalization
369  // purposes only
370 
371  ASSERT_(!PF_options.adaptiveSampleSize);
372 
373  // ----------------------------------------------------------------------
374  // 1) FIXED SAMPLE SIZE VERSION
375  // ----------------------------------------------------------------------
376  newParticles.resize(M);
377  newParticlesWeight.resize(M);
378  newParticlesDerivedFromIdx.resize(M);
379 
380  bool doResample = LMH->ESS() < 0.5;
381 
382  for (i = 0; i < M; i++)
383  {
384  // Generate a new particle:
385  // (a) Draw a "t-1" m_particles' index:
386  // ----------------------------------------------------------------
387  if (doResample)
388  k = LMH->fastDrawSample(
389  PF_options); // Based on weights of last step only!
390  else
391  k = i;
392 
393  oldPose = CPose2D(*LMH->getCurrentPose(k));
394 
395  // (b) Rejection-sampling: Draw a new robot pose from x[k],
396  // and accept it with probability p(zk|x) / maxLikelihood:
397  // ----------------------------------------------------------------
398  if (LMH->m_SFs.empty())
399  {
400  // The first robot pose in the SLAM execution: All m_particles start
401  // at the same point (this is the lowest bound of subsequent
402  // uncertainty):
403  movementDraw = CPose2D(0, 0, 0);
404  newPose = oldPose; // + movementDraw;
405  }
406  else
407  {
408  // Rejection-sampling:
409  do
410  {
411  // Draw new robot pose:
412  if (!maxLikMovementDrawHasBeenUsed[k])
413  {
414  // No! first take advantage of a good drawn value, but only
415  // once!!
416  maxLikMovementDrawHasBeenUsed[k] = true;
417  movementDraw = LMH->m_movementDrawMaximumLikelihood[k];
418 #if 0
419  cout << "Drawn pose (max. lik): " << movementDraw << endl;
420 #endif
421  }
422  else
423  {
424  // Draw new robot pose:
425  // robotMovement->drawSingleSample( movementDraw );
426  // robotMovement->fastDrawSingleSample( movementDraw );
427  movementDraw =
428  LMH->m_movementDraws
429  [LMH->m_movementDrawsIdx++ % size_movementDraws];
430  }
431 
432  newPose.composeFrom(
433  oldPose,
434  movementDraw); // newPose = oldPose + movementDraw;
435 
436  // Compute acceptance probability:
437  newPoseLikelihood = auxiliarComputeObservationLikelihood(
438  PF_options, LMH, k, sf, &newPose);
439  ratioLikLik = exp(newPoseLikelihood - LMH->m_maxLikelihood[k]);
440  acceptanceProb = min(1.0, ratioLikLik);
441 
442  if (ratioLikLik > 1)
443  {
444  if (ratioLikLik > 1.5)
445  {
446  statsWarningsAccProbAboveOne++;
447  // DEBUG
448  // printf("[pfAuxiliaryPFOptimal] Warning!!
449  // p(z|x)/p(z|x*)=%f\n",ratioLikLik);
450  }
451  LMH->m_maxLikelihood[k] = newPoseLikelihood; // :'-( !!!
452  acceptanceProb = 0; // Keep searching!!
453  }
454 
455  statsTrialsCount++; // Stats
456 
457  } while (acceptanceProb < getRandomGenerator().drawUniform(
458  MIN_ACCEPT_UNIF_DISTRIB, 0.999f));
459 
460  statsTrialsSuccess++; // Stats:
461  }
462 
463  // Insert the new particle!:
464  newParticles[i] = newPose;
465 
466  // And its weight:
467  double weightFact =
468  LMH->m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.powFactor;
469 
470  // Add to historic record of log_w weights:
471  LMH->m_log_w_metric_history.resize(M);
472  LMH->m_log_w_metric_history[i][currentPoseID] += weightFact;
473 
474  if (doResample)
475  newParticlesWeight[i] = 0;
476  else
477  newParticlesWeight[i] = LMH->m_particles[k].log_w + weightFact;
478 
479  // and its heritance:
480  newParticlesDerivedFromIdx[i] = (unsigned int)k;
481 
482  } // for i
483 
484  // ---------------------------------------------------------------------------------
485  // Substitute old by new particle set:
486  // Old are in "m_particles"
487  // New are in "newParticles",
488  // "newParticlesWeight","newParticlesDerivedFromIdx"
489  // ---------------------------------------------------------------------------------
490  N = newParticles.size();
491  CLocalMetricHypothesis::CParticleList newParticlesArray(N);
492  CLocalMetricHypothesis::CParticleList::iterator newPartIt, trgPartIt;
493 
494  // For efficiency, just copy the "CRBPFParticleData" from the old particle
495  // into the
496  // new one, but this can be done only once:
497  std::vector<bool> oldParticleAlreadyCopied(LMH->m_particles.size(), false);
498  CLSLAMParticleData* newPartData;
499 
500  for (newPartIt = newParticlesArray.begin(), i = 0;
501  newPartIt != newParticlesArray.end(); newPartIt++, i++)
502  {
503  // The weight:
504  newPartIt->log_w = newParticlesWeight[i];
505 
506  // The data (CRBPFParticleData):
507  if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
508  {
509  // The first copy of this old particle:
510  newPartData =
511  LMH->m_particles[newParticlesDerivedFromIdx[i]].d.release();
512  oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] = true;
513  }
514  else
515  {
516  // Make a copy:
517  newPartData = new CLSLAMParticleData(
518  *LMH->m_particles[newParticlesDerivedFromIdx[i]].d);
519  }
520 
521  newPartIt->d.reset(newPartData);
522  } // end for "newPartIt"
523 
524  // Now add the new robot pose to the paths: (this MUST be done after the
525  // above loop, separately):
526  for (newPartIt = newParticlesArray.begin(), i = 0;
527  newPartIt != newParticlesArray.end(); newPartIt++, i++)
528  newPartIt->d->robotPoses[LMH->m_currentRobotPose] =
529  CPose3D(newParticles[i]);
530 
531  // Free those old m_particles not being copied into the new ones:
532  for (i = 0; i < LMH->m_particles.size(); i++)
533  {
534  LMH->m_particles[i].d.reset();
535  }
536 
537  // Copy into "m_particles":
538  LMH->m_particles.resize(newParticlesArray.size());
539  for (newPartIt = newParticlesArray.begin(),
540  trgPartIt = LMH->m_particles.begin();
541  newPartIt != newParticlesArray.end(); newPartIt++, trgPartIt++)
542  {
543  trgPartIt->log_w = newPartIt->log_w;
544  trgPartIt->d = std::move(newPartIt->d);
545  }
546 
547  // Free buffers:
548  newParticles.clear();
549  newParticlesArray.clear();
550  newParticlesWeight.clear();
551  newParticlesDerivedFromIdx.clear();
552 
553  double out_max_log_w;
554  LMH->normalizeWeights(&out_max_log_w); // Normalize weights:
555  LMH->m_log_w += out_max_log_w;
556 
557 #if 0
558  printf("[REJ-SAMP.RATIO: \t%.03f%% \t %u out of %u with P(acep)>1]\n\n",
559  100.0f*statsTrialsSuccess / (float)(max(1u,statsTrialsCount)),
560  statsWarningsAccProbAboveOne,
561  statsTrialsCount
562  );
563 #endif
564 
565  MRPT_END
566 }
567 
568 /*---------------------------------------------------------------
569  particlesEvaluator_AuxPFOptimal
570  ---------------------------------------------------------------*/
573  const CParticleFilterCapable* obj, size_t index, const void* action,
574  const void* observation)
575 {
576  MRPT_UNUSED_PARAM(action);
577 
578  MRPT_START
579 
580  const auto* myObj = static_cast<const CLocalMetricHypothesis*>(obj);
581 
582  // Compute the quantity:
583  // w[i]p(zt|z^{t-1},x^{[i],t-1})
584  // As the Monte-Carlo approximation of the
585  // integral over all posible $x_t$.
586 
587  // Take the previous particle weight:
588  // --------------------------------------------
589  double indivLik, maxLik = -std::numeric_limits<double>::max();
590  size_t maxLikDraw = 0;
591  size_t N;
592  size_t nDraws = myObj->m_movementDraws.size();
593 
594  ASSERT_(nDraws > 1);
595 
596  // , perform a Monte-Carlo approximation:
597  // --------------------------------------------
599  ASSERT_(N > 1);
600 
601  CPose2D oldPose(*myObj->getCurrentPose(index));
602  // CPose2D drawnSample;
603  mrpt::math::CVectorDouble vectLiks(
604  N, 0); // The vector with the individual log-likelihoods.
605 
606  for (size_t q = 0; q < N; q++)
607  {
608  CPose2D x_predict(
609  oldPose +
610  myObj->m_movementDraws[(++myObj->m_movementDrawsIdx) % nDraws]);
611 
612  // Estimate the mean...
614  PF_options, obj, index, ((CSensoryFrame*)observation), &x_predict);
615 
616  MRPT_CHECK_NORMAL_NUMBER(indivLik);
617  vectLiks[q] = indivLik;
618 
619  // and the maximum value:
620  if (indivLik > maxLik)
621  {
622  maxLikDraw = myObj->m_movementDrawsIdx % nDraws;
623  maxLik = indivLik;
624  }
625  }
626 
627  // This is done to avoid floating point overflow!!
628  double maxLogLik = math::maximum(vectLiks);
629  vectLiks.array() -=
630  maxLogLik; // Maximum log-lik = 0 (max linear likelihood=1)
631 
632  // average_lik = \sum(e^liks) * e^maxLik / N
633  // log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )
634  double avrgLogLik =
635  log(vectLiks.array().exp().sum()) + maxLogLik - log((double)N);
636 
637  // Save into the object:
638  myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index] =
639  avrgLogLik; // log( accum / N );
640  myObj->m_maxLikelihood[index] = maxLik;
641  myObj->m_movementDrawMaximumLikelihood[index] =
642  myObj->m_movementDraws[maxLikDraw];
643 
644  // and compute the resulting probability of this particle:
645  // ------------------------------------------------------------
646  // if (myObj->m_adaptiveSampleSize)
647  // return myObj->m_particles[index].w *
648  // myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
649  // else return myObj->m_particles[index].w;
650 
651  double ret = myObj->m_particles[index].log_w +
652  myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
653 
655 
656  return ret;
657 
658  MRPT_END
659 }
660 
661 /*---------------------------------------------------------------
662  auxiliarComputeObservationLikelihood
663  ---------------------------------------------------------------*/
666  const CParticleFilterCapable* obj, size_t particleIndexForMap,
667  const CSensoryFrame* observation, const CPose2D* x)
668 {
669  MRPT_UNUSED_PARAM(PF_options);
670  const auto* theObj = static_cast<const CLocalMetricHypothesis*>(obj);
671  auto* map = const_cast<CMultiMetricMap*>(
672  &theObj->m_particles[particleIndexForMap].d->metricMaps);
673 
674  return map->computeObservationsLikelihood(*observation, *x);
675 }
676 
677 /*---------------------------------------------------------------
678  dumpToStdOut
679  ---------------------------------------------------------------*/
681 {
682  std::vector<int>::const_iterator it;
683 
684  std::cout << "x = [";
685  for (it = x.begin(); it != x.end(); it++) std::cout << *it << " ";
686  std::cout << "]" << std::endl;
687 
688  std::cout << "y = [";
689  for (it = y.begin(); it != y.end(); it++) std::cout << *it << " ";
690  std::cout << "]" << std::endl;
691 
692  std::cout << "Phi = [";
693  for (it = phi.begin(); it != phi.end(); it++) std::cout << *it << " ";
694  std::cout << "]" << std::endl;
695 }
696 
697 /*---------------------------------------------------------------
698  loadTPathBinFromPath
699  ---------------------------------------------------------------*/
702  CPose2D* newPose)
703 {
704  size_t lenBinPath;
705 
706  if (path != nullptr)
707  lenBinPath = path->size();
708  else
709  lenBinPath = 0;
710 
711  TMapPoseID2Pose3D::const_iterator itSrc;
712  std::vector<int>::iterator itX, itY, itPHI;
713 
714  // Set the output bin dimensionality:
715  outBin.x.resize(lenBinPath + (newPose != nullptr ? 1 : 0));
716  outBin.y.resize(lenBinPath + (newPose != nullptr ? 1 : 0));
717  outBin.phi.resize(lenBinPath + (newPose != nullptr ? 1 : 0));
718 
719  // Is a path provided??
720  if (path != nullptr)
721  {
722  // Fill the bin data:
723  for (itSrc = path->begin(), itX = outBin.x.begin(),
724  itY = outBin.y.begin(), itPHI = outBin.phi.begin();
725  itSrc != path->end(); itSrc++, itX++, itY++, itPHI++)
726  {
727  *itX = (int)round(
728  itSrc->second.x() /
729  m_parent->m_options.KLD_params.KLD_binSize_XY);
730  *itY = (int)round(
731  itSrc->second.y() /
732  m_parent->m_options.KLD_params.KLD_binSize_XY);
733  *itPHI = (int)round(
734  itSrc->second.yaw() /
735  m_parent->m_options.KLD_params.KLD_binSize_PHI);
736  } // end-for build path bin
737  }
738 
739  // Is a newPose provided??
740  if (newPose != nullptr)
741  {
742  // And append the last pose: the new one:
743  outBin.x[lenBinPath] = (int)round(
744  newPose->x() / m_parent->m_options.KLD_params.KLD_binSize_XY);
745  outBin.y[lenBinPath] = (int)round(
746  newPose->y() / m_parent->m_options.KLD_params.KLD_binSize_XY);
747  outBin.phi[lenBinPath] = (int)round(
748  newPose->phi() / m_parent->m_options.KLD_params.KLD_binSize_PHI);
749  }
750 }
751 
752 /*---------------------------------------------------------------
753  findTPathBinIntoSet
754  ---------------------------------------------------------------*/
756  CLSLAM_RBPF_2DLASER::TPathBin& desiredBin,
757  std::deque<CLSLAM_RBPF_2DLASER::TPathBin>& theSet)
758 {
759  // it = pathBins.find( p ); <---- This doesn't work!!!
760  // TODO: A more efficient search algorithm here!!
761  std::deque<CLSLAM_RBPF_2DLASER::TPathBin>::iterator it;
762  int ret;
763 
764  for (it = theSet.begin(), ret = 0; it != theSet.end(); it++, ret++)
765  if ((it->x == desiredBin.x) && (it->y == desiredBin.y) &&
766  (it->phi == desiredBin.phi))
767  return ret;
768 
769  // Not found!
770  return -1;
771 }
772 
773 /** The PF algorithm implementation for "optimal sampling" approximated with
774  * scan matching (Stachniss method)
775  */
778  const mrpt::obs::CSensoryFrame* sf,
780 {
781  MRPT_START
782 
783  CTicTac tictac;
784 
785  // Get the current robot pose estimation:
786  TPoseID currentPoseID = LMH->m_currentRobotPose;
787 
788  // ----------------------------------------------------------------------
789  // We can execute optimal PF only when we have both, an action, and
790  // a valid observation from which to compute the likelihood:
791  // Accumulate odometry/actions until we have a valid observation, then
792  // process them simultaneously.
793  // ----------------------------------------------------------------------
794  bool SFhasValidObservations = false;
795  // A valid action?
796  if (actions != nullptr)
797  {
799  actions->getBestMovementEstimation(); // Find a robot movement
800  // estimation:
801  if (!act)
803  "Action list does not contain any CActionRobotMovement2D "
804  "derived object!");
805 
806  if (!LMH->m_accumRobotMovementIsValid) // Reset accum.
807  {
808  act->poseChange->getMean(
811  act->motionModelConfiguration;
812  }
813  else
816  act->poseChange->getMeanVal();
817 
818  LMH->m_accumRobotMovementIsValid = true;
819  }
820 
821  if (sf != nullptr)
822  {
823  ASSERT_(LMH->m_particles.size() > 0);
824  SFhasValidObservations =
825  (*LMH->m_particles.begin())
826  .d->metricMaps.canComputeObservationsLikelihood(*sf);
827  }
828 
829  // All the needed things?
830  if (!LMH->m_accumRobotMovementIsValid || !SFhasValidObservations)
831  return; // Nothing we can do here...
832  ASSERT_(sf != nullptr);
833  ASSERT_(!PF_options.adaptiveSampleSize);
834 
835  // OK, we have all we need, let's start!
836 
837  // The odometry-based increment since last step is
838  // in: LMH->m_accumRobotMovement.rawOdometryIncrementReading
839  const CPose2D initialPoseEstimation =
842  false; // To reset odometry at next iteration!
843 
844  // ----------------------------------------------------------------------
845  // 1) FIXED SAMPLE SIZE VERSION
846  // ----------------------------------------------------------------------
847 
848  // ICP used if "pfOptimalProposal_mapSelection" = 0 or 1
849  CICP icp;
850  CICP::TReturnInfo icpInfo;
851 
852  // ICP options
853  // ------------------------------
854  icp.options.maxIterations = 80;
855  icp.options.thresholdDist = 0.50f;
856  icp.options.thresholdAng = DEG2RAD(20);
857  icp.options.smallestThresholdDist = 0.05f;
858  icp.options.ALFA = 0.5f;
859  icp.options.doRANSAC = false;
860 
861  // Build the map of points to align:
862  CSimplePointsMap localMapPoints;
863 
864  ASSERT_(
865  LMH->m_particles[0]
866  .d->metricMaps.countMapsByClass<COccupancyGridMap2D>() > 0);
867 
868  // Build local map:
869  localMapPoints.clear();
870  localMapPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f;
871  sf->insertObservationsInto(&localMapPoints);
872 
873  // Process the particles
874  const size_t M = LMH->m_particles.size();
875  LMH->m_log_w_metric_history.resize(M);
876 
877  for (size_t i = 0; i < M; i++)
878  {
880  CPose3D* part_pose = LMH->getCurrentPose(i);
881 
882  if (LMH->m_SFs.empty())
883  {
884  // The first robot pose in the SLAM execution: All m_particles start
885  // at the same point (this is the lowest bound of subsequent
886  // uncertainty):
887  // New pose = old pose.
888  // part_pose: Unmodified
889  }
890  else
891  {
892  // ICP and add noise:
893  CPosePDFGaussian icpEstimation;
894 
895  // Select map to use with ICP:
896  CMetricMap* mapalign;
897  auto& mMap = part.d->metricMaps;
898 
899  if (auto pPts = mMap.mapByClass<CSimplePointsMap>(); pPts)
900  mapalign = pPts.get();
901  else if (auto pGrid = mMap.mapByClass<COccupancyGridMap2D>(); pGrid)
902  mapalign = pGrid.get();
903  else
905  "There is no point or grid map. At least one needed for "
906  "ICP.");
907 
908  // Use ICP to align to each particle's map:
909  CPosePDF::Ptr alignEst = icp.Align(
910  mapalign, &localMapPoints, initialPoseEstimation, nullptr,
911  &icpInfo);
912 
913  icpEstimation.copyFrom(*alignEst);
914 
915  // Generate gaussian-distributed 2D-pose increments according to
916  // "finalEstimatedPoseGauss":
917  // -------------------------------------------------------------------------------------------
918  // Set the gaussian pose:
919  CPose3DPDFGaussian finalEstimatedPoseGauss(icpEstimation);
920 
921  CPose3D noisy_increment;
922  finalEstimatedPoseGauss.drawSingleSample(noisy_increment);
923 
924  CPose3D new_pose;
925  new_pose.composeFrom(*part_pose, noisy_increment);
926 
927  CPose2D new_pose2d = CPose2D(new_pose);
928 
929  // Add the pose to the path:
930  part.d->robotPoses[LMH->m_currentRobotPose] = new_pose;
931 
932  // Update the weight:
933  // ---------------------------------------------------------------------------
934  const double log_lik =
936  PF_options, LMH, i, sf, &new_pose2d);
937 
938  part.log_w += log_lik;
939 
940  // Add to historic record of log_w weights:
941  LMH->m_log_w_metric_history[i][currentPoseID] += log_lik;
942 
943  } // end else we can do ICP
944 
945  } // end for each particle i
946 
947  // Accumulate the log likelihood of this LMH as a whole:
948  double out_max_log_w;
949  LMH->normalizeWeights(&out_max_log_w); // Normalize weights:
950  LMH->m_log_w += out_max_log_w;
951 
952  printf("[CLSLAM_RBPF_2DLASER] Overall likelihood = %.2e\n", out_max_log_w);
953  printf("[CLSLAM_RBPF_2DLASER] Done in %.03fms\n", 1e3 * tictac.Tac());
954 
955  MRPT_END
956 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
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.
std::vector< mrpt::poses::CPose2D > m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
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) ...
#define MRPT_START
Definition: exceptions.h:241
#define min(a, b)
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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.
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...
std::vector< double > m_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
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.
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
double yaw() const
Get the YAW angle (in radians)
Definition: CPose3D.h:542
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.
Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
Definition: CHMTSLAM.h:516
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
Definition: CICP.h:120
TConfigParams options
The options employed by the ICP align.
Definition: CICP.h:180
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
Definition: CHMTSLAM.h:67
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.
double distanceTo(const CPoseOrPoint< OTHERCLASS, DIM2 > &b) const
Returns the Euclidean distance to another pose/point:
Definition: CPoseOrPoint.h:214
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
~CLSLAM_RBPF_2DLASER() override
Destructor.
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
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...
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:125
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:563
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
#define POSEID_INVALID
GLuint index
Definition: glext.h:4068
mrpt::safe_ptr< CHMTSLAM > m_parent
Definition: CHMTSLAM.h:521
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:66
std::map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
unsigned int maxIterations
Maximum number of iterations to run.
Definition: CICP.h:101
CONTAINER::Scalar maximum(const CONTAINER &v)
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:51
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:133
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
void processOneLMH(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection::Ptr &act, const mrpt::obs::CSensoryFrame::Ptr &sf) override
Main entry point from HMT-SLAM: process some actions & observations.
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).
void prediction_and_update_pfAuxiliaryPFOptimal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
The PF algorithm implementation.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:50
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...
CONTAINER::Scalar minimum(const CONTAINER &v)
A class for storing an occupancy grid map.
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.
std::vector< mrpt::poses::CPose2D > m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
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:84
double ALFA
The scale factor for thresholds everytime convergence is achieved.
Definition: CICP.h:117
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:86
The configuration of a particle filter.
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
Definition: CPose2D.cpp:135
#define MRPT_END
Definition: exceptions.h:245
The ICP algorithm return information.
Definition: CICP.h:190
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:3542
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:271
void resize(std::size_t N, bool zeroNewElements=false)
GLenum GLint x
Definition: glext.h:3542
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
This class stores any customizable set of metric maps.
bool m_insertNewRobotPose
For use within PF callback methods.
Definition: CHMTSLAM.h:612
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:114
void prediction_and_update_pfOptimalProposal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
The PF algorithm implementation.
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:232
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: c26d46ba6 Thu Jul 18 13:03:42 2019 +0200 at jue jul 18 13:10:17 CEST 2019