MRPT  2.0.4
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-2020, 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,
574  [[maybe_unused]] const void* action, const void* observation)
575 {
576  MRPT_START
577 
578  const auto* myObj = static_cast<const CLocalMetricHypothesis*>(obj);
579 
580  // Compute the quantity:
581  // w[i]p(zt|z^{t-1},x^{[i],t-1})
582  // As the Monte-Carlo approximation of the
583  // integral over all posible $x_t$.
584 
585  // Take the previous particle weight:
586  // --------------------------------------------
587  double indivLik, maxLik = -std::numeric_limits<double>::max();
588  size_t maxLikDraw = 0;
589  size_t N;
590  size_t nDraws = myObj->m_movementDraws.size();
591 
592  ASSERT_(nDraws > 1);
593 
594  // , perform a Monte-Carlo approximation:
595  // --------------------------------------------
597  ASSERT_(N > 1);
598 
599  CPose2D oldPose(*myObj->getCurrentPose(index));
600  // CPose2D drawnSample;
601  mrpt::math::CVectorDouble vectLiks(
602  N, 0); // The vector with the individual log-likelihoods.
603 
604  for (size_t q = 0; q < N; q++)
605  {
606  CPose2D x_predict(
607  oldPose +
608  myObj->m_movementDraws[(++myObj->m_movementDrawsIdx) % nDraws]);
609 
610  // Estimate the mean...
612  PF_options, obj, index, ((CSensoryFrame*)observation), &x_predict);
613 
614  MRPT_CHECK_NORMAL_NUMBER(indivLik);
615  vectLiks[q] = indivLik;
616 
617  // and the maximum value:
618  if (indivLik > maxLik)
619  {
620  maxLikDraw = myObj->m_movementDrawsIdx % nDraws;
621  maxLik = indivLik;
622  }
623  }
624 
625  // This is done to avoid floating point overflow!!
626  double maxLogLik = math::maximum(vectLiks);
627  vectLiks.array() -=
628  maxLogLik; // Maximum log-lik = 0 (max linear likelihood=1)
629 
630  // average_lik = \sum(e^liks) * e^maxLik / N
631  // log( average_lik ) = log( \sum(e^liks) ) + maxLik - log( N )
632  double avrgLogLik =
633  log(vectLiks.array().exp().sum()) + maxLogLik - log((double)N);
634 
635  // Save into the object:
636  myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index] =
637  avrgLogLik; // log( accum / N );
638  myObj->m_maxLikelihood[index] = maxLik;
639  myObj->m_movementDrawMaximumLikelihood[index] =
640  myObj->m_movementDraws[maxLikDraw];
641 
642  // and compute the resulting probability of this particle:
643  // ------------------------------------------------------------
644  // if (myObj->m_adaptiveSampleSize)
645  // return myObj->m_particles[index].w *
646  // myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
647  // else return myObj->m_particles[index].w;
648 
649  double ret = myObj->m_particles[index].log_w +
650  myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
651 
653 
654  return ret;
655 
656  MRPT_END
657 }
658 
659 /*---------------------------------------------------------------
660  auxiliarComputeObservationLikelihood
661  ---------------------------------------------------------------*/
663  [[maybe_unused]] const bayes::CParticleFilter::TParticleFilterOptions&
664  PF_options,
665  const CParticleFilterCapable* obj, size_t particleIndexForMap,
666  const CSensoryFrame* observation, const CPose2D* x)
667 {
668  const auto* theObj = static_cast<const CLocalMetricHypothesis*>(obj);
669  auto* map = const_cast<CMultiMetricMap*>(
670  &theObj->m_particles[particleIndexForMap].d->metricMaps);
671 
672  return map->computeObservationsLikelihood(*observation, *x);
673 }
674 
675 /*---------------------------------------------------------------
676  dumpToStdOut
677  ---------------------------------------------------------------*/
679 {
680  std::vector<int>::const_iterator it;
681 
682  std::cout << "x = [";
683  for (it = x.begin(); it != x.end(); it++) std::cout << *it << " ";
684  std::cout << "]" << std::endl;
685 
686  std::cout << "y = [";
687  for (it = y.begin(); it != y.end(); it++) std::cout << *it << " ";
688  std::cout << "]" << std::endl;
689 
690  std::cout << "Phi = [";
691  for (it = phi.begin(); it != phi.end(); it++) std::cout << *it << " ";
692  std::cout << "]" << std::endl;
693 }
694 
695 /*---------------------------------------------------------------
696  loadTPathBinFromPath
697  ---------------------------------------------------------------*/
700  CPose2D* newPose)
701 {
702  size_t lenBinPath;
703 
704  if (path != nullptr)
705  lenBinPath = path->size();
706  else
707  lenBinPath = 0;
708 
709  TMapPoseID2Pose3D::const_iterator itSrc;
710  std::vector<int>::iterator itX, itY, itPHI;
711 
712  // Set the output bin dimensionality:
713  outBin.x.resize(lenBinPath + (newPose != nullptr ? 1 : 0));
714  outBin.y.resize(lenBinPath + (newPose != nullptr ? 1 : 0));
715  outBin.phi.resize(lenBinPath + (newPose != nullptr ? 1 : 0));
716 
717  // Is a path provided??
718  if (path != nullptr)
719  {
720  // Fill the bin data:
721  for (itSrc = path->begin(), itX = outBin.x.begin(),
722  itY = outBin.y.begin(), itPHI = outBin.phi.begin();
723  itSrc != path->end(); itSrc++, itX++, itY++, itPHI++)
724  {
725  *itX = (int)round(
726  itSrc->second.x() /
727  m_parent->m_options.KLD_params.KLD_binSize_XY);
728  *itY = (int)round(
729  itSrc->second.y() /
730  m_parent->m_options.KLD_params.KLD_binSize_XY);
731  *itPHI = (int)round(
732  itSrc->second.yaw() /
733  m_parent->m_options.KLD_params.KLD_binSize_PHI);
734  } // end-for build path bin
735  }
736 
737  // Is a newPose provided??
738  if (newPose != nullptr)
739  {
740  // And append the last pose: the new one:
741  outBin.x[lenBinPath] = (int)round(
742  newPose->x() / m_parent->m_options.KLD_params.KLD_binSize_XY);
743  outBin.y[lenBinPath] = (int)round(
744  newPose->y() / m_parent->m_options.KLD_params.KLD_binSize_XY);
745  outBin.phi[lenBinPath] = (int)round(
746  newPose->phi() / m_parent->m_options.KLD_params.KLD_binSize_PHI);
747  }
748 }
749 
750 /*---------------------------------------------------------------
751  findTPathBinIntoSet
752  ---------------------------------------------------------------*/
754  CLSLAM_RBPF_2DLASER::TPathBin& desiredBin,
755  std::deque<CLSLAM_RBPF_2DLASER::TPathBin>& theSet)
756 {
757  // it = pathBins.find( p ); <---- This doesn't work!!!
758  // TODO: A more efficient search algorithm here!!
759  std::deque<CLSLAM_RBPF_2DLASER::TPathBin>::iterator it;
760  int ret;
761 
762  for (it = theSet.begin(), ret = 0; it != theSet.end(); it++, ret++)
763  if ((it->x == desiredBin.x) && (it->y == desiredBin.y) &&
764  (it->phi == desiredBin.phi))
765  return ret;
766 
767  // Not found!
768  return -1;
769 }
770 
771 /** The PF algorithm implementation for "optimal sampling" approximated with
772  * scan matching (Stachniss method)
773  */
776  const mrpt::obs::CSensoryFrame* sf,
778 {
779  MRPT_START
780 
781  CTicTac tictac;
782 
783  // Get the current robot pose estimation:
784  TPoseID currentPoseID = LMH->m_currentRobotPose;
785 
786  // ----------------------------------------------------------------------
787  // We can execute optimal PF only when we have both, an action, and
788  // a valid observation from which to compute the likelihood:
789  // Accumulate odometry/actions until we have a valid observation, then
790  // process them simultaneously.
791  // ----------------------------------------------------------------------
792  bool SFhasValidObservations = false;
793  // A valid action?
794  if (actions != nullptr)
795  {
797  actions->getBestMovementEstimation(); // Find a robot movement
798  // estimation:
799  if (!act)
801  "Action list does not contain any CActionRobotMovement2D "
802  "derived object!");
803 
804  if (!LMH->m_accumRobotMovementIsValid) // Reset accum.
805  {
806  act->poseChange->getMean(
809  act->motionModelConfiguration;
810  }
811  else
814  act->poseChange->getMeanVal();
815 
816  LMH->m_accumRobotMovementIsValid = true;
817  }
818 
819  if (sf != nullptr)
820  {
821  ASSERT_(LMH->m_particles.size() > 0);
822  SFhasValidObservations =
823  (*LMH->m_particles.begin())
824  .d->metricMaps.canComputeObservationsLikelihood(*sf);
825  }
826 
827  // All the needed things?
828  if (!LMH->m_accumRobotMovementIsValid || !SFhasValidObservations)
829  return; // Nothing we can do here...
830  ASSERT_(sf != nullptr);
831  ASSERT_(!PF_options.adaptiveSampleSize);
832 
833  // OK, we have all we need, let's start!
834 
835  // The odometry-based increment since last step is
836  // in: LMH->m_accumRobotMovement.rawOdometryIncrementReading
837  const CPose2D initialPoseEstimation =
840  false; // To reset odometry at next iteration!
841 
842  // ----------------------------------------------------------------------
843  // 1) FIXED SAMPLE SIZE VERSION
844  // ----------------------------------------------------------------------
845 
846  // ICP used if "pfOptimalProposal_mapSelection" = 0 or 1
847  CICP icp;
848  CICP::TReturnInfo icpInfo;
849 
850  // ICP options
851  // ------------------------------
852  icp.options.maxIterations = 80;
853  icp.options.thresholdDist = 0.50f;
854  icp.options.thresholdAng = 20.0_deg;
855  icp.options.smallestThresholdDist = 0.05f;
856  icp.options.ALFA = 0.5f;
857  icp.options.doRANSAC = false;
858 
859  // Build the map of points to align:
860  CSimplePointsMap localMapPoints;
861 
862  ASSERT_(
863  LMH->m_particles[0]
864  .d->metricMaps.countMapsByClass<COccupancyGridMap2D>() > 0);
865 
866  // Build local map:
867  localMapPoints.clear();
868  localMapPoints.insertionOptions.minDistBetweenLaserPoints = 0.02f;
869  sf->insertObservationsInto(&localMapPoints);
870 
871  // Process the particles
872  const size_t M = LMH->m_particles.size();
873  LMH->m_log_w_metric_history.resize(M);
874 
875  for (size_t i = 0; i < M; i++)
876  {
878  CPose3D* part_pose = LMH->getCurrentPose(i);
879 
880  if (LMH->m_SFs.empty())
881  {
882  // The first robot pose in the SLAM execution: All m_particles start
883  // at the same point (this is the lowest bound of subsequent
884  // uncertainty):
885  // New pose = old pose.
886  // part_pose: Unmodified
887  }
888  else
889  {
890  // ICP and add noise:
891  CPosePDFGaussian icpEstimation;
892 
893  // Select map to use with ICP:
894  CMetricMap* mapalign;
895  auto& mMap = part.d->metricMaps;
896 
897  if (auto pPts = mMap.mapByClass<CSimplePointsMap>(); pPts)
898  mapalign = pPts.get();
899  else if (auto pGrid = mMap.mapByClass<COccupancyGridMap2D>(); pGrid)
900  mapalign = pGrid.get();
901  else
903  "There is no point or grid map. At least one needed for "
904  "ICP.");
905 
906  // Use ICP to align to each particle's map:
907  CPosePDF::Ptr alignEst = icp.Align(
908  mapalign, &localMapPoints, initialPoseEstimation, icpInfo);
909 
910  icpEstimation.copyFrom(*alignEst);
911 
912  // Generate gaussian-distributed 2D-pose increments according to
913  // "finalEstimatedPoseGauss":
914  // -------------------------------------------------------------------------------------------
915  // Set the gaussian pose:
916  CPose3DPDFGaussian finalEstimatedPoseGauss(icpEstimation);
917 
918  CPose3D noisy_increment;
919  finalEstimatedPoseGauss.drawSingleSample(noisy_increment);
920 
921  CPose3D new_pose;
922  new_pose.composeFrom(*part_pose, noisy_increment);
923 
924  CPose2D new_pose2d = CPose2D(new_pose);
925 
926  // Add the pose to the path:
927  part.d->robotPoses[LMH->m_currentRobotPose] = new_pose;
928 
929  // Update the weight:
930  // ---------------------------------------------------------------------------
931  const double log_lik =
933  PF_options, LMH, i, sf, &new_pose2d);
934 
935  part.log_w += log_lik;
936 
937  // Add to historic record of log_w weights:
938  LMH->m_log_w_metric_history[i][currentPoseID] += log_lik;
939 
940  } // end else we can do ICP
941 
942  } // end for each particle i
943 
944  // Accumulate the log likelihood of this LMH as a whole:
945  double out_max_log_w;
946  LMH->normalizeWeights(&out_max_log_w); // Normalize weights:
947  LMH->m_log_w += out_max_log_w;
948 
949  printf("[CLSLAM_RBPF_2DLASER] Overall likelihood = %.2e\n", out_max_log_w);
950  printf("[CLSLAM_RBPF_2DLASER] Done in %.03fms\n", 1e3 * tictac.Tac());
951 
952  MRPT_END
953 }
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.
mrpt::poses::CPosePDF::Ptr Align(const mrpt::maps::CMetricMap *m1, const mrpt::maps::CMetricMap *m2, const mrpt::poses::CPose2D &grossEst, mrpt::optional_ref< TMetricMapAlignmentResult > outInfo=std::nullopt)
The method for aligning a pair of metric maps, for SE(2) relative poses.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
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
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.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
Definition: CICP.h:64
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.
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:546
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
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...
return_t drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
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:556
double phi() const
Get the phi angle of the 2D pose (in radians)
Definition: CPose2D.h:86
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
#define POSEID_INVALID
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...
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...
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...
const mrpt::poses::CPose3D * getCurrentPose(size_t particleIdx) const
Returns the i&#39;th particle hypothesis for the current robot pose.
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:85
double ALFA
The scale factor for thresholds every time 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 ...
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 ...
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
Definition: CPointsMap.h:274
void resize(std::size_t N, bool zeroNewElements=false)
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76
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.
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:235
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020