47 static constexpr
bool DoesResampling =
false;
48 MRPT_TODO(
"MOVE predicition_and_update code here");
54 static constexpr
bool DoesResampling =
true;
55 MRPT_TODO(
"MOVE prediction_and_update here");
58 void CLocalMetricHypothesis::executeOn(
66 case CParticleFilter::pfOptimalProposal:
69 *
this, action, observation, stats);
71 case CParticleFilter::pfAuxiliaryPFOptimal:
74 *
this, action, observation, stats);
88 CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER(
CHMTSLAM* parent)
132 it->d->robotPoses[currentPoseID] = initPose;
142 firstArea->m_annotations.setElemental(
147 bool insertNewRobotPose =
false;
155 insertNewRobotPose =
true;
164 CPose3D* currentRobotPose = &lstRobotPoses[currentPoseID];
165 float minDistLin = 1e6f;
166 float minDistAng = 1e6f;
170 it != lstRobotPoses.end(); ++it)
172 if (it->first != currentPoseID)
174 float linDist = it->second.
distanceTo(*currentRobotPose);
176 it->second.yaw() - currentRobotPose->
yaw()));
178 minDistLin =
min(minDistLin, linDist);
180 if (linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS)
181 minDistAng =
min(minDistAng, angDist);
187 (minDistLin >
m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) ||
188 (minDistAng >
m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS);
212 if (insertNewRobotPose)
215 mrpt::utils::LVL_INFO,
216 "[CLSLAM_RBPF_2DLASER] Adding new pose...\n");
231 const CPose3D* curRobotPose = &partIt->d->robotPoses[currentPoseID];
232 partIt->d->robotPoses[newCurrentPoseID] = *curRobotPose;
233 sf->insertObservationsInto(&partIt->d->metricMaps, curRobotPose);
242 LMH->
m_SFs[currentPoseID] = *sf;
246 TPoseID newlyAddedPose = currentPoseID;
256 mrpt::utils::LVL_INFO,
"[CLSLAM_RBPF_2DLASER] Added pose %i.\n",
257 (
int)newlyAddedPose);
262 std::lock_guard<std::mutex> lock(
m_parent->m_topLCdets_cs);
266 it !=
m_parent->m_topLCdets.end(); ++it)
267 (*it)->OnNewPose(newlyAddedPose, sf.get());
279 void CLocalMetricHypothesis::prediction_and_update<LSLAMAuxiliaryPFOptimal>(
287 TPoseID currentPoseID = m_currentRobotPose;
289 size_t i, k, N, M = m_poseParticles.m_particles.size();
299 bool SFhasValidObservations =
false;
301 if (actions !=
nullptr)
304 actions->getBestMovementEstimation();
308 "Action list does not contain any CActionRobotMovement2D " 311 if (!m_accumRobotMovementIsValid)
313 act->poseChange->getMean(
314 m_accumRobotMovement.rawOdometryIncrementReading);
315 m_accumRobotMovement.motionModelConfiguration =
316 act->motionModelConfiguration;
319 m_accumRobotMovement.rawOdometryIncrementReading =
320 m_accumRobotMovement.rawOdometryIncrementReading +
321 act->poseChange->getMeanVal();
323 m_accumRobotMovementIsValid =
true;
328 ASSERT_(m_poseParticles.m_particles.size() > 0);
329 SFhasValidObservations =
330 (*m_poseParticles.m_particles.begin())
331 .d->metricMaps.canComputeObservationsLikelihood(*sf);
335 if (!m_accumRobotMovementIsValid || !SFhasValidObservations)
345 m_accumRobotMovement.motionModelConfiguration.gaussianModel
347 m_parent->m_options.MIN_ODOMETRY_STD_XY);
349 m_accumRobotMovement.motionModelConfiguration.gaussianModel
351 m_parent->m_options.MIN_ODOMETRY_STD_PHI);
354 m_accumRobotMovement.rawOdometryIncrementReading,
355 m_accumRobotMovement.motionModelConfiguration);
359 m_accumRobotMovementIsValid =
366 m_movementDraws.clear();
372 m_movementDraws.resize(
373 PF_options.pfAuxFilterOptimal_MaximumSearchSamples * M);
374 size_t size_movementDraws = m_movementDraws.size();
375 m_movementDrawsIdx = (
unsigned int)floor(
379 for (
size_t i = 0; i < m_movementDraws.size(); i++)
382 m_pfAuxiliaryPFOptimal_estimatedProb.resize(M);
383 m_maxLikelihood.clear();
384 m_maxLikelihood.resize(M, 0);
385 m_movementDrawMaximumLikelihood.resize(M);
390 using namespace std::placeholders;
391 m_poseParticles.prepareFastDrawSample(
392 PF_options, [&](
size_t i)
394 return this->particlesEvaluator_AuxPFOptimal(PF_options, i, sf);
396 printf(
"[prepareFastDrawSample] Done in %.06f ms\n", tictac.
Tac() * 1e3f);
399 printf(
"[prepareFastDrawSample] max (log) = %10.06f\n",
math::maximum(m_poseParticles.m_pfAuxiliaryPFOptimal_estimatedProb) );
400 printf(
"[prepareFastDrawSample] max-mean (log) = %10.06f\n", -
math::mean(m_poseParticles.m_pfAuxiliaryPFOptimal_estimatedProb) +
math::maximum(m_poseParticles.m_pfAuxiliaryPFOptimal_estimatedProb) );
401 printf(
"[prepareFastDrawSample] max-min (log) = %10.06f\n", -
math::minimum(m_poseParticles.m_pfAuxiliaryPFOptimal_estimatedProb) +
math::maximum(m_poseParticles.m_pfAuxiliaryPFOptimal_estimatedProb) );
410 vector<double> newParticlesWeight;
411 vector<size_t> newParticlesDerivedFromIdx;
420 float MIN_ACCEPT_UNIF_DISTRIB = 0.00f;
422 CPose2D movementDraw, newPose, oldPose;
423 double acceptanceProb, newPoseLikelihood, ratioLikLik;
424 unsigned int statsTrialsCount = 0, statsTrialsSuccess = 0;
425 std::vector<bool> maxLikMovementDrawHasBeenUsed(M,
false);
426 unsigned int statsWarningsAccProbAboveOne = 0;
431 ASSERT_(!PF_options.adaptiveSampleSize);
436 newParticles.resize(M);
437 newParticlesWeight.resize(M);
438 newParticlesDerivedFromIdx.resize(M);
440 bool doResample = m_poseParticles.ESS() < 0.5;
442 for (i = 0; i < M; i++)
448 k = m_poseParticles.fastDrawSample(
453 oldPose =
CPose2D(*getCurrentPose(k));
463 movementDraw =
CPose2D(0, 0, 0);
472 if (!maxLikMovementDrawHasBeenUsed[k])
476 maxLikMovementDrawHasBeenUsed[k] =
true;
477 movementDraw = m_movementDrawMaximumLikelihood[k];
479 cout <<
"Drawn pose (max. lik): " << movementDraw << endl;
488 m_movementDraws[m_movementDrawsIdx++ % size_movementDraws];
496 newPoseLikelihood = auxiliarComputeObservationLikelihood(
497 PF_options, k, sf, &newPose);
498 ratioLikLik = exp(newPoseLikelihood - m_maxLikelihood[k]);
499 acceptanceProb =
min(1.0, ratioLikLik);
503 if (ratioLikLik > 1.5)
505 statsWarningsAccProbAboveOne++;
510 m_maxLikelihood[k] = newPoseLikelihood;
517 MIN_ACCEPT_UNIF_DISTRIB, 0.999f));
519 statsTrialsSuccess++;
523 newParticles[i] = newPose;
527 m_pfAuxiliaryPFOptimal_estimatedProb[k] * PF_options.powFactor;
530 m_log_w_metric_history.
resize(M);
531 m_log_w_metric_history[i][currentPoseID] += weightFact;
534 newParticlesWeight[i] = 0;
536 newParticlesWeight[i] = m_poseParticles.m_particles[k].log_w + weightFact;
539 newParticlesDerivedFromIdx[i] = (
unsigned int)k;
549 N = newParticles.size();
556 std::vector<bool> oldParticleAlreadyCopied(m_poseParticles.m_particles.size(),
false);
559 for (newPartIt = newParticlesArray.begin(), i = 0;
560 newPartIt != newParticlesArray.end(); newPartIt++, i++)
563 newPartIt->log_w = newParticlesWeight[i];
566 if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
570 m_poseParticles.m_particles[newParticlesDerivedFromIdx[i]].d.release();
571 oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] =
true;
577 *m_poseParticles.m_particles[newParticlesDerivedFromIdx[i]].d);
580 newPartIt->d.reset(newPartData);
585 for (newPartIt = newParticlesArray.begin(), i = 0;
586 newPartIt != newParticlesArray.end(); newPartIt++, i++)
587 newPartIt->d->robotPoses[m_currentRobotPose] =
591 for (i = 0; i < m_poseParticles.m_particles.size(); i++)
593 m_poseParticles.m_particles[i].d.reset();
597 m_poseParticles.m_particles.
resize(newParticlesArray.size());
598 for (newPartIt = newParticlesArray.begin(),
599 trgPartIt = m_poseParticles.m_particles.begin();
600 newPartIt != newParticlesArray.end(); newPartIt++, trgPartIt++)
602 trgPartIt->log_w = newPartIt->log_w;
603 trgPartIt->d.move_from(newPartIt->d);
607 newParticles.clear();
608 newParticlesArray.clear();
609 newParticlesWeight.clear();
610 newParticlesDerivedFromIdx.clear();
612 double out_max_log_w;
613 m_poseParticles.normalizeWeights(&out_max_log_w);
614 m_log_w += out_max_log_w;
617 printf(
"[REJ-SAMP.RATIO: \t%.03f%% \t %u out of %u with P(acep)>1]\n\n",
618 100.0f*statsTrialsSuccess / (
float)(max(1u,statsTrialsCount)),
619 statsWarningsAccProbAboveOne,
643 double indivLik, maxLik = -std::numeric_limits<double>::max();
644 size_t maxLikDraw = 0;
660 for (
size_t q = 0;
q < N;
q++)
668 PF_options,
index, observation, &x_predict);
671 vectLiks[
q] = indivLik;
674 if (indivLik > maxLik)
689 log(vectLiks.array().exp().sum()) + maxLogLik - log((
double)N);
720 size_t particleIndexForMap,
737 std::cout <<
"x = [";
738 for (it =
x.begin(); it !=
x.end(); it++) std::cout << *it <<
" ";
739 std::cout <<
"]" << std::endl;
741 std::cout <<
"y = [";
742 for (it =
y.begin(); it !=
y.end(); it++) std::cout << *it <<
" ";
743 std::cout <<
"]" << std::endl;
745 std::cout <<
"Phi = [";
746 for (it =
phi.begin(); it !=
phi.end(); it++) std::cout << *it <<
" ";
747 std::cout <<
"]" << std::endl;
760 lenBinPath = path->size();
768 outBin.
x.resize(lenBinPath + (newPose !=
nullptr ? 1 : 0));
769 outBin.
y.resize(lenBinPath + (newPose !=
nullptr ? 1 : 0));
770 outBin.
phi.resize(lenBinPath + (newPose !=
nullptr ? 1 : 0));
776 for (itSrc = path->begin(), itX = outBin.
x.begin(),
777 itY = outBin.
y.begin(), itPHI = outBin.
phi.begin();
778 itSrc != path->end(); itSrc++, itX++, itY++, itPHI++)
782 m_parent->m_options.KLD_params.KLD_binSize_XY);
785 m_parent->m_options.KLD_params.KLD_binSize_XY);
787 itSrc->second.yaw() /
788 m_parent->m_options.KLD_params.KLD_binSize_PHI);
793 if (newPose !=
nullptr)
796 outBin.
x[lenBinPath] = (int)
round(
797 newPose->
x() /
m_parent->m_options.KLD_params.KLD_binSize_XY);
798 outBin.
y[lenBinPath] = (int)
round(
799 newPose->
y() /
m_parent->m_options.KLD_params.KLD_binSize_XY);
800 outBin.
phi[lenBinPath] = (int)
round(
801 newPose->
phi() /
m_parent->m_options.KLD_params.KLD_binSize_PHI);
810 std::deque<CLSLAM_RBPF_2DLASER::TPathBin>& theSet)
817 for (it = theSet.begin(), ret = 0; it != theSet.end(); it++, ret++)
818 if ((it->x == desiredBin.
x) && (it->y == desiredBin.
y) &&
819 (it->phi == desiredBin.
phi))
830 void CLocalMetricHypothesis::prediction_and_update<LSLAMOptimalProposal>(
840 TPoseID currentPoseID = m_currentRobotPose;
848 bool SFhasValidObservations =
false;
850 if (actions !=
nullptr)
853 actions->getBestMovementEstimation();
857 "Action list does not contain any CActionRobotMovement2D " 860 if (!m_accumRobotMovementIsValid)
862 act->poseChange->getMean(
863 m_accumRobotMovement.rawOdometryIncrementReading);
864 m_accumRobotMovement.motionModelConfiguration =
865 act->motionModelConfiguration;
868 m_accumRobotMovement.rawOdometryIncrementReading =
869 m_accumRobotMovement.rawOdometryIncrementReading +
870 act->poseChange->getMeanVal();
872 m_accumRobotMovementIsValid =
true;
877 ASSERT_(m_poseParticles.m_particles.size() > 0);
878 SFhasValidObservations =
879 (*m_poseParticles.m_particles.begin())
880 .d->metricMaps.canComputeObservationsLikelihood(*sf);
884 if (!m_accumRobotMovementIsValid || !SFhasValidObservations)
887 ASSERT_(!PF_options.adaptiveSampleSize);
893 const CPose2D initialPoseEstimation =
894 m_accumRobotMovement.rawOdometryIncrementReading;
895 m_accumRobotMovementIsValid =
919 ASSERT_(m_poseParticles.m_particles[0].d->metricMaps.m_gridMaps.size() > 0);
924 localMapPoints.
clear();
926 sf->insertObservationsInto(&localMapPoints);
929 const size_t M = m_poseParticles.m_particles.size();
930 m_log_w_metric_history.resize(M);
932 for (
size_t i = 0; i < M; i++)
935 CPose3D* part_pose = getCurrentPose(i);
953 if (!part.
d->metricMaps.m_pointsMaps.empty())
954 mapalign = part.
d->metricMaps.m_pointsMaps[0].get();
955 else if (!part.
d->metricMaps.m_gridMaps.empty())
956 mapalign = part.
d->metricMaps.m_gridMaps[0].get();
959 "There is no point or grid map. At least one needed for " 964 mapalign, &localMapPoints, initialPoseEstimation,
nullptr,
971 CPose3D Ap = finalEstimatedPoseGauss.mean - ith_last_pose;
972 double Ap_dist = Ap.
norm();
973 finalEstimatedPoseGauss.cov.zeros();
974 finalEstimatedPoseGauss.cov(0,0) =
square( fabs(Ap_dist)*0.01 );
975 finalEstimatedPoseGauss.cov(1,1) =
square( fabs(Ap_dist)*0.01 );
976 finalEstimatedPoseGauss.cov(2,2) =
square( fabs(Ap.
yaw())*0.02 );
994 part.
d->robotPoses[m_currentRobotPose] = new_pose;
998 const double log_lik =
999 PF_options.powFactor * auxiliarComputeObservationLikelihood(
1000 PF_options, i, sf, &new_pose2d);
1002 part.
log_w += log_lik;
1005 m_log_w_metric_history[i][currentPoseID] += log_lik;
1012 double out_max_log_w;
1013 m_poseParticles.normalizeWeights(&out_max_log_w);
1014 m_log_w += out_max_log_w;
1016 printf(
"[CLSLAM_RBPF_2DLASER] Overall likelihood = %.2e\n", out_max_log_w);
1017 printf(
"[CLSLAM_RBPF_2DLASER] Done in %.03fms\n", 1e3 * tictac.Tac());
void clear()
Erase all the contents of the map.
A namespace of pseudo-random numbers genrators of diferent distributions.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
double x() const
Common members of all points & poses classes.
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) ...
void dumpToStdOut() const
For debugging purposes!
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
float thresholdDist
Initial threshold distance for two points to become a correspondence.
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 ...
double particlesEvaluator_AuxPFOptimal(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, size_t index, const mrpt::obs::CSensoryFrame *observation)
Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".
#define MRPT_CHECK_NORMAL_NUMBER(val)
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.
#define THROW_EXCEPTION(msg)
GLdouble GLdouble GLdouble GLdouble q
This file implements several operations that operate element-wise on individual or pairs of container...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
double yaw() const
Get the YAW angle (in radians)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
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
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
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...
float ALFA
The scale factor for threshold everytime convergence is achieved.
Statistics for being returned from the "execute" method.
Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
TConfigParams options
The options employed by the ICP align.
void Tic()
Starts the stopwatch.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
void executeOn(mrpt::bayes::CParticleFilter &pf, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, mrpt::bayes::CParticleFilter::TParticleFilterStats *stats, mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm PF_algorithm)
Declares a class for storing a collection of robot actions.
mrpt::utils::safe_ptr< CHMTSLAM > m_parent
T square(const T x)
Inline function for the square of a number.
mrpt::poses::StdVector_CPose2D m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
CONTAINER::Scalar minimum(const CONTAINER &v)
Represents a probabilistic 2D movement of the robot mobile base.
CParticleList m_particles
The array of particles.
std::shared_ptr< CPosePDF > Ptr
mrpt::aligned_containers< TPoseID, mrpt::poses::CPose3D >::map_t TMapPoseID2Pose3D
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 ...
std::shared_ptr< CActionRobotMovement2D > Ptr
#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...
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
double norm() const
Returns the euclidean norm of vector: .
mrpt::bayes::CParticleFilterData< CLSLAMParticleData >::CParticleList CParticleList
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...
unsigned int maxIterations
Maximum number of iterations to run.
This class implements a high-performance stopwatch.
static void resize(const size_t n)
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...
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
std::shared_ptr< CSensoryFrame > Ptr
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...
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).
bool onlyClosestCorrespondences
The usual approach: to consider only the closest correspondence for each local point (Default to true...
CONTAINER::Scalar maximum(const CONTAINER &v)
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
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...
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 prepareFastDrawSingleSamples() const
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "dr...
void executeOn(PARTICLEFILTERCAPABLE &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.
mrpt::poses::StdVector_CPose2D m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
double auxiliarComputeObservationLikelihood(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, 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...
void processOneLMH(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection::Ptr &act, const mrpt::obs::CSensoryFrame::Ptr &sf)
Main entry point from HMT-SLAM: process some actions & observations.
std::shared_ptr< CActionCollection > Ptr
TParticleFilterAlgorithm
Defines different types of particle filter algorithms.
A template class for holding a the data and the weight of a particle.
void getMeans(TMapPoseID2Pose3D &outList) const
Returns the mean of each robot pose in this LMH, as computed from the set of particles.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
float smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
The configuration of a particle filter.
static void resize(const size_t n)
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
double log_w
The (logarithmic) weight value for this particle.
The ICP algorithm return information.
mrpt::utils::copy_ptr< T > d
The data associated with this particle.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
double Tac()
Stops the stopwatch.
#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.
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
std::shared_ptr< CHMHMapNode > Ptr
This class stores any customizable set of metric maps.
bool m_insertNewRobotPose
For use within PF callback methods.
CLSLAMParticleDataParticles m_poseParticles
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.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
virtual ~CLSLAM_RBPF_2DLASER()
Destructor.
const mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx) const
Returns the i'th particle hypothesis for the current robot pose.
TParticleFilterAlgorithm PF_algorithm
The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilitie...
mrpt::aligned_containers< CPose2D >::vector_t StdVector_CPose2D
Eigen aligment-compatible container.
float minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.