Go to the documentation of this file.
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 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.
This class is used in HMT-SLAM to represent each of the Local Metric Hypotheses (LMHs).
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
THypothesisID m_ID
The unique ID of the hypothesis (Used for accessing mrpt::slam::CHierarchicalMHMap).
TConfigParams options
The options employed by the ICP align.
double smallestThresholdDist
The size for threshold such that iterations will stop, since it is considered precise enough.
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
double ALFA
The scale factor for thresholds everytime convergence is achieved.
const Scalar * const_iterator
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.
double thresholdDist
Initial threshold distance for two points to become a correspondence.
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...
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
CONTAINER::Scalar maximum(const CONTAINER &v)
GLdouble GLdouble GLdouble GLdouble q
std::shared_ptr< CSensoryFrame > Ptr
bool m_accumRobotMovementIsValid
Used in CLSLAM_RBPF_2DLASER.
A high-performance stopwatch, with typical resolution of nanoseconds.
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.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
An implementation of Hybrid Metric Topological SLAM (HMT-SLAM).
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter.
static TPoseID generatePoseID()
Generates a new and unique pose ID.
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 minDistBetweenLaserPoints
The minimum distance between points (in 3D): If two points are too close, one of them is not inserted...
mrpt::aligned_std_vector< mrpt::poses::CPose2D > m_movementDraws
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
double mean(const CONTAINER &v)
Computes the mean value of a vector.
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
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).
#define NODE_ANNOTATION_REF_POSEID
const double & phi() const
Get the phi angle of the 2D pose (in radians)
GLsizei GLsizei GLuint * obj
double distanceTo(const CPoseOrPoint< OTHERCLASS > &b) const
Returns the Euclidean distance to another pose/point:
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement
Used in CLSLAM_RBPF_2DLASER.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
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...
Auxiliary class used in mrpt::slam::CLocalMetricHypothesis for HMT-SLAM; this class keeps the data re...
Declares a class for storing a collection of robot actions.
double norm() const
Returns the euclidean norm of vector: .
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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...
Represents a probabilistic 2D movement of the robot mobile base.
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
T square(const T x)
Inline function for the square of a number.
std::shared_ptr< CActionRobotMovement2D > Ptr
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void dumpToStdOut() const
For debugging purposes!
TPoseIDList m_posesPendingAddPartitioner
The list of poseIDs waiting to be added to the graph partitioner, what happens in the LSLAM thread ma...
This namespace contains representation of robot actions and observations.
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise.
The ICP algorithm return information.
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 Tac() noexcept
Stops the stopwatch.
A template class for holding a the data and the weight of a particle.
TPoseID m_currentRobotPose
The current robot pose (its global unique ID) for this hypothesis.
unsigned int m_movementDrawsIdx
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
std::map< TPoseID, mrpt::obs::CSensoryFrame > m_SFs
The SF gathered at each robot pose.
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,...
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Classes related to the implementation of Hybrid Metric Topological (HMT) SLAM.
Declares a virtual base class for all metric maps storage classes.
This virtual class defines the interface that any particles based PDF class must implement in order t...
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm,...
mrpt::poses::CPose2D rawOdometryIncrementReading
This is the raw odometry reading, and only is used when "estimationMethod" is "TEstimationMethod::emO...
mrpt::aligned_std_map< TPoseID, mrpt::poses::CPose3D > TMapPoseID2Pose3D
std::map< TPoseID, CHMHMapNode::TNodeID > m_nodeIDmemberships
The hybrid map node membership for each robot pose.
int round(const T value)
Returns the closer integer (int) to x.
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.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
This class stores any customizable set of metric maps.
double x() const
Common members of all points & poses classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
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.
mrpt::safe_ptr< CHMTSLAM > m_parent
void fastDrawSingleSample(mrpt::poses::CPose2D &outSample) const
Faster version than "drawSingleSample", but requires a previous call to "prepareFastDrawSingleSamples...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Virtual base for local SLAM methods, used in mrpt::slam::CHMTSLAM.
void Tic() noexcept
Starts the stopwatch.
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions::TOptions_GaussianModel gaussianModel
Several implementations of ICP (Iterative closest point) algorithms for aligning two point maps or a ...
CActionRobotMovement2D::Ptr getBestMovementEstimation() const
Returns the best pose increment estimator in the collection, based on the determinant of its pose cha...
double m_log_w
Log-weight of this hypothesis.
CParticleList m_particles
The array of particles.
uint64_t TPoseID
An integer number uniquely identifying each robot pose stored in HMT-SLAM.
mrpt::aligned_std_vector< mrpt::poses::CPose2D > m_movementDrawMaximumLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
const mrpt::poses::CPose3D * getCurrentPose(const size_t &particleIdx) const
Returns the i'th particle hypothesis for the current robot pose.
TInsertionOptions insertionOptions
The options used when inserting observations in the map.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
unsigned int maxIterations
Maximum number of iterations to run.
std::shared_ptr< CHMHMapNode > Ptr
virtual ~CLSLAM_RBPF_2DLASER()
Destructor.
bool doRANSAC
Perform a RANSAC step, mrpt::tfest::se2_l2_robust(), after the ICP convergence, to obtain a better es...
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
double yaw() const
Get the YAW angle (in radians)
void prepareFastDrawSingleSamples() const
Call this before calling a high number of times "fastDrawSingleSample", which is much faster than "dr...
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::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
A namespace of pseudo-random numbers generators of diferent distributions.
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.
void clear()
Erase all the contents of the map.
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
std::vector< double > m_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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::shared_ptr< CActionCollection > Ptr
CONTAINER::Scalar minimum(const CONTAINER &v)
std::shared_ptr< CPosePDF > Ptr
struct mrpt::obs::CActionRobotMovement2D::TMotionModelOptions motionModelConfiguration
The configuration of a particle filter.
mrpt::safe_ptr< CHMTSLAM > m_parent
For quick access to our parent object.
double ESS() const override
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
bool m_insertNewRobotPose
For use within PF callback methods.
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...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
double normalizeWeights(double *out_max_log_w=nullptr) override
Normalize the (logarithmic) weights, such as the maximum weight is zero.
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".
void getMeans(TMapPoseID2Pose3D &outList) const
Returns the mean of each robot pose in this LMH, as computed from the set of particles.
double DEG2RAD(const double x)
Degrees to radians.
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |