23 #include <Eigen/Dense> 37 CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER(
CHMTSLAM* parent)
79 m_particle.d->robotPoses[currentPoseID] = initPose;
89 firstArea->m_annotations.setElemental(
94 bool insertNewRobotPose =
false;
102 insertNewRobotPose =
true;
111 CPose3D* currentRobotPose = &lstRobotPoses[currentPoseID];
112 float minDistLin = 1e6f;
113 float minDistAng = 1e6f;
116 for (
auto& lstRobotPose : lstRobotPoses)
118 if (lstRobotPose.first != currentPoseID)
121 lstRobotPose.second.
distanceTo(*currentRobotPose);
123 lstRobotPose.second.yaw() - currentRobotPose->
yaw()));
125 minDistLin = min(minDistLin, linDist);
127 if (linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS)
128 minDistAng = min(minDistAng, angDist);
134 (minDistLin >
m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) ||
135 (minDistAng >
m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS);
151 pf.
executeOn(*LMH, actions.get(), sf.get());
159 if (insertNewRobotPose)
163 "[CLSLAM_RBPF_2DLASER] Adding new pose...\n");
177 &m_particle.d->robotPoses[currentPoseID];
178 m_particle.d->robotPoses[newCurrentPoseID] = *curRobotPose;
179 sf->insertObservationsInto(&m_particle.d->metricMaps, curRobotPose);
188 LMH->
m_SFs[currentPoseID] = *sf;
192 TPoseID newlyAddedPose = currentPoseID;
203 (
int)newlyAddedPose);
208 std::lock_guard<std::mutex> lock(
m_parent->m_topLCdets_cs);
210 for (
auto& m_topLCdet :
m_parent->m_topLCdets)
211 m_topLCdet->OnNewPose(newlyAddedPose, sf.get());
242 bool SFhasValidObservations =
false;
244 if (actions !=
nullptr)
251 "Action list does not contain any CActionRobotMovement2D " 256 act->poseChange->getMean(
259 act->motionModelConfiguration;
264 act->poseChange->getMeanVal();
272 SFhasValidObservations =
274 .d->metricMaps.canComputeObservationsLikelihood(*sf);
290 LMH->
m_parent->m_options.MIN_ODOMETRY_STD_XY);
294 LMH->
m_parent->m_options.MIN_ODOMETRY_STD_PHI);
320 0.0f, ((
float)size_movementDraws) - 0.01f));
336 printf(
"[prepareFastDrawSample] Done in %.06f ms\n", tictac.
Tac() * 1e3f);
349 std::vector<mrpt::poses::CPose2D> newParticles;
350 vector<double> newParticlesWeight;
351 vector<size_t> newParticlesDerivedFromIdx;
360 float MIN_ACCEPT_UNIF_DISTRIB = 0.00f;
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;
376 newParticles.resize(M);
377 newParticlesWeight.resize(M);
378 newParticlesDerivedFromIdx.resize(M);
380 bool doResample = LMH->
ESS() < 0.5;
382 for (i = 0; i < M; i++)
398 if (LMH->
m_SFs.empty())
403 movementDraw =
CPose2D(0, 0, 0);
412 if (!maxLikMovementDrawHasBeenUsed[k])
416 maxLikMovementDrawHasBeenUsed[k] =
true;
419 cout <<
"Drawn pose (max. lik): " << movementDraw << endl;
438 PF_options, LMH, k, sf, &newPose);
440 acceptanceProb = min(1.0, ratioLikLik);
444 if (ratioLikLik > 1.5)
446 statsWarningsAccProbAboveOne++;
458 MIN_ACCEPT_UNIF_DISTRIB, 0.999f));
460 statsTrialsSuccess++;
464 newParticles[i] = newPose;
475 newParticlesWeight[i] = 0;
477 newParticlesWeight[i] = LMH->
m_particles[k].log_w + weightFact;
480 newParticlesDerivedFromIdx[i] = (
unsigned int)k;
490 N = newParticles.size();
492 CLocalMetricHypothesis::CParticleList::iterator newPartIt, trgPartIt;
497 std::vector<bool> oldParticleAlreadyCopied(LMH->
m_particles.size(),
false);
500 for (newPartIt = newParticlesArray.begin(), i = 0;
501 newPartIt != newParticlesArray.end(); newPartIt++, i++)
504 newPartIt->log_w = newParticlesWeight[i];
507 if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
511 LMH->
m_particles[newParticlesDerivedFromIdx[i]].d.release();
512 oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] =
true;
518 *LMH->
m_particles[newParticlesDerivedFromIdx[i]].d);
521 newPartIt->d.reset(newPartData);
526 for (newPartIt = newParticlesArray.begin(), i = 0;
527 newPartIt != newParticlesArray.end(); newPartIt++, i++)
539 for (newPartIt = newParticlesArray.begin(),
541 newPartIt != newParticlesArray.end(); newPartIt++, trgPartIt++)
543 trgPartIt->log_w = newPartIt->log_w;
544 trgPartIt->d = std::move(newPartIt->d);
548 newParticles.clear();
549 newParticlesArray.clear();
550 newParticlesWeight.clear();
551 newParticlesDerivedFromIdx.clear();
553 double out_max_log_w;
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,
574 [[maybe_unused]]
const void* action,
const void* observation)
587 double indivLik, maxLik = -std::numeric_limits<double>::max();
588 size_t maxLikDraw = 0;
590 size_t nDraws = myObj->m_movementDraws.size();
599 CPose2D oldPose(*myObj->getCurrentPose(index));
604 for (
size_t q = 0; q < N; q++)
608 myObj->m_movementDraws[(++myObj->m_movementDrawsIdx) % nDraws]);
612 PF_options, obj, index, ((
CSensoryFrame*)observation), &x_predict);
615 vectLiks[q] = indivLik;
618 if (indivLik > maxLik)
620 maxLikDraw = myObj->m_movementDrawsIdx % nDraws;
633 log(vectLiks.array().exp().sum()) + maxLogLik - log((
double)N);
636 myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index] =
638 myObj->m_maxLikelihood[index] = maxLik;
639 myObj->m_movementDrawMaximumLikelihood[index] =
640 myObj->m_movementDraws[maxLikDraw];
649 double ret = myObj->m_particles[index].log_w +
650 myObj->m_pfAuxiliaryPFOptimal_estimatedProb[index];
670 &theObj->m_particles[particleIndexForMap].d->metricMaps);
680 std::vector<int>::const_iterator it;
682 std::cout <<
"x = [";
683 for (it =
x.begin(); it !=
x.end(); it++) std::cout << *it <<
" ";
684 std::cout <<
"]" << std::endl;
686 std::cout <<
"y = [";
687 for (it =
y.begin(); it !=
y.end(); it++) std::cout << *it <<
" ";
688 std::cout <<
"]" << std::endl;
690 std::cout <<
"Phi = [";
691 for (it =
phi.begin(); it !=
phi.end(); it++) std::cout << *it <<
" ";
692 std::cout <<
"]" << std::endl;
705 lenBinPath = path->size();
709 TMapPoseID2Pose3D::const_iterator itSrc;
710 std::vector<int>::iterator itX, itY, itPHI;
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));
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++)
727 m_parent->m_options.KLD_params.KLD_binSize_XY);
730 m_parent->m_options.KLD_params.KLD_binSize_XY);
732 itSrc->second.yaw() /
733 m_parent->m_options.KLD_params.KLD_binSize_PHI);
738 if (newPose !=
nullptr)
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);
755 std::deque<CLSLAM_RBPF_2DLASER::TPathBin>& theSet)
759 std::deque<CLSLAM_RBPF_2DLASER::TPathBin>::iterator it;
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))
792 bool SFhasValidObservations =
false;
794 if (actions !=
nullptr)
801 "Action list does not contain any CActionRobotMovement2D " 806 act->poseChange->getMean(
809 act->motionModelConfiguration;
814 act->poseChange->getMeanVal();
822 SFhasValidObservations =
824 .d->metricMaps.canComputeObservationsLikelihood(*sf);
837 const CPose2D initialPoseEstimation =
867 localMapPoints.
clear();
875 for (
size_t i = 0; i < M; i++)
880 if (LMH->
m_SFs.empty())
895 auto& mMap = part.d->metricMaps;
898 mapalign = pPts.get();
900 mapalign = pGrid.get();
903 "There is no point or grid map. At least one needed for " 908 mapalign, &localMapPoints, initialPoseEstimation, icpInfo);
931 const double log_lik =
933 PF_options, LMH, i, sf, &new_pose2d);
935 part.log_w += log_lik;
945 double out_max_log_w;
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());
void clear()
Erase all the contents of the map.
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.
double minStdPHI
Additional uncertainty: [degrees].
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) ...
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
#define THROW_EXCEPTION(msg)
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 ...
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...
TMotionModelOptions motionModelConfiguration
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 minStdXY
Additional uncertainty: [meters].
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. ...
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...
Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough...
TConfigParams options
The options employed by the ICP align.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
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:
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
void dumpToStdOut() const
For debugging purposes!
~CLSLAM_RBPF_2DLASER() override
Destructor.
#define ASSERT_(f)
Defines an assertion mechanism.
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...
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...
double phi() const
Get the phi angle of the 2D pose (in radians)
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
mrpt::safe_ptr< CHMTSLAM > m_parent
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...
std::map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
unsigned int maxIterations
Maximum number of iterations to run.
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...
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...
double x() const
Common members of all points & poses classes.
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.
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'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.
TOptions_GaussianModel gaussianModel
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.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
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).
double ALFA
The scale factor for thresholds every time convergence is achieved.
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 .
The ICP algorithm return information.
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.
void resize(std::size_t N, bool zeroNewElements=false)
void Tic() noexcept
Starts the stopwatch.
This class stores any customizable set of metric maps.
bool m_insertNewRobotPose
For use within PF callback methods.
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.
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...
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.