38 CLSLAM_RBPF_2DLASER::CLSLAM_RBPF_2DLASER(
CHMTSLAM* parent)
82 it->d->robotPoses[currentPoseID] = initPose;
92 firstArea->m_annotations.setElemental(
97 bool insertNewRobotPose =
false;
105 insertNewRobotPose =
true;
114 CPose3D* currentRobotPose = &lstRobotPoses[currentPoseID];
115 float minDistLin = 1e6f;
116 float minDistAng = 1e6f;
120 it != lstRobotPoses.end(); ++it)
122 if (it->first != currentPoseID)
124 float linDist = it->second.
distanceTo(*currentRobotPose);
125 float angDist = fabs(
127 it->second.yaw() - currentRobotPose->
yaw()));
129 minDistLin =
min(minDistLin, linDist);
131 if (linDist < m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS)
132 minDistAng =
min(minDistAng, angDist);
138 (minDistLin >
m_parent->m_options.SLAM_MIN_DIST_BETWEEN_OBS) ||
139 (minDistAng >
m_parent->m_options.SLAM_MIN_HEADING_BETWEEN_OBS);
155 pf.
executeOn(*LMH, actions.get(), sf.get());
163 if (insertNewRobotPose)
167 "[CLSLAM_RBPF_2DLASER] Adding new pose...\n");
182 const CPose3D* curRobotPose = &partIt->d->robotPoses[currentPoseID];
183 partIt->d->robotPoses[newCurrentPoseID] = *curRobotPose;
184 sf->insertObservationsInto(&partIt->d->metricMaps, curRobotPose);
193 LMH->
m_SFs[currentPoseID] = *sf;
197 TPoseID newlyAddedPose = currentPoseID;
208 (
int)newlyAddedPose);
213 std::lock_guard<std::mutex> lock(
m_parent->m_topLCdets_cs);
217 it !=
m_parent->m_topLCdets.end(); ++it)
218 (*it)->OnNewPose(newlyAddedPose, sf.get());
249 bool SFhasValidObservations =
false;
251 if (actions !=
nullptr)
258 "Action list does not contain any CActionRobotMovement2D " 263 act->poseChange->getMean(
266 act->motionModelConfiguration;
271 act->poseChange->getMeanVal();
279 SFhasValidObservations =
281 .d->metricMaps.canComputeObservationsLikelihood(*sf);
297 LMH->
m_parent->m_options.MIN_ODOMETRY_STD_XY);
301 LMH->
m_parent->m_options.MIN_ODOMETRY_STD_PHI);
327 0.0f, ((
float)size_movementDraws) - 0.01f));
343 printf(
"[prepareFastDrawSample] Done in %.06f ms\n", tictac.
Tac() * 1e3f);
357 vector<double> newParticlesWeight;
358 vector<size_t> newParticlesDerivedFromIdx;
367 float MIN_ACCEPT_UNIF_DISTRIB = 0.00f;
369 CPose2D movementDraw, newPose, oldPose;
370 double acceptanceProb, newPoseLikelihood, ratioLikLik;
371 unsigned int statsTrialsCount = 0, statsTrialsSuccess = 0;
372 std::vector<bool> maxLikMovementDrawHasBeenUsed(M,
false);
373 unsigned int statsWarningsAccProbAboveOne = 0;
383 newParticles.resize(M);
384 newParticlesWeight.resize(M);
385 newParticlesDerivedFromIdx.resize(M);
387 bool doResample = LMH->
ESS() < 0.5;
389 for (i = 0; i < M; i++)
405 if (LMH->
m_SFs.empty())
410 movementDraw =
CPose2D(0, 0, 0);
419 if (!maxLikMovementDrawHasBeenUsed[k])
423 maxLikMovementDrawHasBeenUsed[k] =
true;
426 cout <<
"Drawn pose (max. lik): " << movementDraw << endl;
445 PF_options, LMH, k, sf, &newPose);
447 acceptanceProb =
min(1.0, ratioLikLik);
451 if (ratioLikLik > 1.5)
453 statsWarningsAccProbAboveOne++;
465 MIN_ACCEPT_UNIF_DISTRIB, 0.999f));
467 statsTrialsSuccess++;
471 newParticles[i] = newPose;
482 newParticlesWeight[i] = 0;
484 newParticlesWeight[i] = LMH->
m_particles[k].log_w + weightFact;
487 newParticlesDerivedFromIdx[i] = (
unsigned int)k;
497 N = newParticles.size();
504 std::vector<bool> oldParticleAlreadyCopied(LMH->
m_particles.size(),
false);
507 for (newPartIt = newParticlesArray.begin(), i = 0;
508 newPartIt != newParticlesArray.end(); newPartIt++, i++)
511 newPartIt->log_w = newParticlesWeight[i];
514 if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
518 LMH->
m_particles[newParticlesDerivedFromIdx[i]].d.release();
519 oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] =
true;
525 *LMH->
m_particles[newParticlesDerivedFromIdx[i]].d);
528 newPartIt->d.reset(newPartData);
533 for (newPartIt = newParticlesArray.begin(), i = 0;
534 newPartIt != newParticlesArray.end(); newPartIt++, i++)
546 for (newPartIt = newParticlesArray.begin(),
548 newPartIt != newParticlesArray.end(); newPartIt++, trgPartIt++)
550 trgPartIt->log_w = newPartIt->log_w;
551 trgPartIt->d.move_from(newPartIt->d);
555 newParticles.clear();
556 newParticlesArray.clear();
557 newParticlesWeight.clear();
558 newParticlesDerivedFromIdx.clear();
560 double out_max_log_w;
565 printf(
"[REJ-SAMP.RATIO: \t%.03f%% \t %u out of %u with P(acep)>1]\n\n",
566 100.0f*statsTrialsSuccess / (
float)(max(1u,statsTrialsCount)),
567 statsWarningsAccProbAboveOne,
581 const void* observation)
597 double indivLik, maxLik = -std::numeric_limits<double>::max();
598 size_t maxLikDraw = 0;
600 size_t nDraws = myObj->m_movementDraws.size();
614 for (
size_t q = 0;
q < N;
q++)
618 myObj->m_movementDraws[(++myObj->m_movementDrawsIdx) % nDraws]);
625 vectLiks[
q] = indivLik;
628 if (indivLik > maxLik)
630 maxLikDraw = myObj->m_movementDrawsIdx % nDraws;
643 log(vectLiks.array().exp().sum()) + maxLogLik - log((
double)N);
646 myObj->m_pfAuxiliaryPFOptimal_estimatedProb[
index] =
648 myObj->m_maxLikelihood[
index] = maxLik;
649 myObj->m_movementDrawMaximumLikelihood[
index] =
650 myObj->m_movementDraws[maxLikDraw];
659 double ret = myObj->m_particles[
index].log_w +
660 myObj->m_pfAuxiliaryPFOptimal_estimatedProb[
index];
681 &theObj->
m_particles[particleIndexForMap].d->metricMaps);
693 std::cout <<
"x = [";
694 for (it =
x.begin(); it !=
x.end(); it++) std::cout << *it <<
" ";
695 std::cout <<
"]" << std::endl;
697 std::cout <<
"y = [";
698 for (it =
y.begin(); it !=
y.end(); it++) std::cout << *it <<
" ";
699 std::cout <<
"]" << std::endl;
701 std::cout <<
"Phi = [";
702 for (it =
phi.begin(); it !=
phi.end(); it++) std::cout << *it <<
" ";
703 std::cout <<
"]" << std::endl;
716 lenBinPath = path->size();
724 outBin.
x.resize(lenBinPath + (newPose !=
nullptr ? 1 : 0));
725 outBin.
y.resize(lenBinPath + (newPose !=
nullptr ? 1 : 0));
726 outBin.
phi.resize(lenBinPath + (newPose !=
nullptr ? 1 : 0));
732 for (itSrc = path->begin(), itX = outBin.
x.begin(),
733 itY = outBin.
y.begin(), itPHI = outBin.
phi.begin();
734 itSrc != path->end(); itSrc++, itX++, itY++, itPHI++)
738 m_parent->m_options.KLD_params.KLD_binSize_XY);
741 m_parent->m_options.KLD_params.KLD_binSize_XY);
743 itSrc->second.yaw() /
744 m_parent->m_options.KLD_params.KLD_binSize_PHI);
749 if (newPose !=
nullptr)
752 outBin.
x[lenBinPath] = (int)
round(
753 newPose->
x() /
m_parent->m_options.KLD_params.KLD_binSize_XY);
754 outBin.
y[lenBinPath] = (int)
round(
755 newPose->
y() /
m_parent->m_options.KLD_params.KLD_binSize_XY);
756 outBin.
phi[lenBinPath] = (int)
round(
757 newPose->
phi() /
m_parent->m_options.KLD_params.KLD_binSize_PHI);
766 std::deque<CLSLAM_RBPF_2DLASER::TPathBin>& theSet)
773 for (it = theSet.begin(), ret = 0; it != theSet.end(); it++, ret++)
774 if ((it->x == desiredBin.
x) && (it->y == desiredBin.
y) &&
775 (it->phi == desiredBin.
phi))
803 bool SFhasValidObservations =
false;
805 if (actions !=
nullptr)
812 "Action list does not contain any CActionRobotMovement2D " 817 act->poseChange->getMean(
820 act->motionModelConfiguration;
825 act->poseChange->getMeanVal();
833 SFhasValidObservations =
835 .d->metricMaps.canComputeObservationsLikelihood(*sf);
848 const CPose2D initialPoseEstimation =
878 localMapPoints.
clear();
886 for (
size_t i = 0; i < M; i++)
891 if (LMH->
m_SFs.empty())
907 if (!part.d->metricMaps.m_pointsMaps.empty())
908 mapalign = part.d->metricMaps.m_pointsMaps[0].get();
909 else if (!part.d->metricMaps.m_gridMaps.empty())
910 mapalign = part.d->metricMaps.m_gridMaps[0].get();
913 "There is no point or grid map. At least one needed for " 918 mapalign, &localMapPoints, initialPoseEstimation,
nullptr,
925 CPose3D Ap = finalEstimatedPoseGauss.mean - ith_last_pose;
926 double Ap_dist = Ap.
norm();
927 finalEstimatedPoseGauss.cov.zeros();
928 finalEstimatedPoseGauss.cov(0,0) =
square( fabs(Ap_dist)*0.01 );
929 finalEstimatedPoseGauss.cov(1,1) =
square( fabs(Ap_dist)*0.01 );
930 finalEstimatedPoseGauss.cov(2,2) =
square( fabs(Ap.
yaw())*0.02 );
952 const double log_lik =
954 PF_options, LMH, i, sf, &new_pose2d);
956 part.log_w += log_lik;
966 double out_max_log_w;
970 printf(
"[CLSLAM_RBPF_2DLASER] Overall likelihood = %.2e\n", out_max_log_w);
971 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.
double Tac() noexcept
Stops the stopwatch.
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) ...
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:
#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.
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.
mrpt::aligned_std_vector< mrpt::poses::CPose2D > m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
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
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.
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.
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
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. ...
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...
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
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.
GLsizei GLsizei GLuint * obj
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.
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
T square(const T x)
Inline function for the square of a number.
void dumpToStdOut() const
For debugging purposes!
CONTAINER::Scalar minimum(const CONTAINER &v)
#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...
mrpt::aligned_std_map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
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...
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::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...
unsigned int maxIterations
Maximum number of iterations to run.
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...
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).
CONTAINER::Scalar maximum(const CONTAINER &v)
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...
virtual ~CLSLAM_RBPF_2DLASER()
Destructor.
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...
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel gaussianModel
mrpt::aligned_std_vector< mrpt::poses::CPose2D > m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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.
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 everytime convergence is achieved.
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)
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 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.
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motionModelConfiguration
void prediction_and_update_pfAuxiliaryPFOptimal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
const Scalar * const_iterator
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'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...
void prediction_and_update_pfOptimalProposal(CLocalMetricHypothesis *LMH, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
int round(const T value)
Returns the closer integer (int) to x.