A set of common data shared by PF implementations for both SLAM and localization.
Definition at line 40 of file PF_implementations_data.h.
#include <mrpt/slam/PF_implementations_data.h>
Public Member Functions | |
PF_implementation () | |
template<class BINTYPE > | |
bool | PF_SLAM_implementation_gatherActionsCheckBothActObs (const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf) |
Auxiliary method called by PF implementations: return true if we have both action & observation, otherwise, return false AND accumulate the odometry so when we have an observation we didn't lose a thing. More... | |
Virtual methods that the PF_implementations assume exist. | |
virtual const mrpt::math::TPose3D * | getLastPose (const size_t i) const =0 |
Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). More... | |
virtual void | PF_SLAM_implementation_custom_update_particle_with_new_pose (PARTICLE_TYPE *particleData, const mrpt::math::TPose3D &newPose) const =0 |
virtual void | PF_SLAM_implementation_replaceByNewParticleSet (typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::CParticleList &old_particles, const std::vector< mrpt::math::TPose3D > &newParticles, const std::vector< double > &newParticlesWeight, const std::vector< size_t > &newParticlesDerivedFromIdx) const |
This is the default algorithm to efficiently replace one old set of samples by another new set. More... | |
virtual bool | PF_SLAM_implementation_doWeHaveValidObservations (const typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const |
virtual bool | PF_SLAM_implementation_skipRobotMovement () const |
Make a specialization if needed, eg. More... | |
virtual double | PF_SLAM_computeObservationLikelihoodForParticle (const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const size_t particleIndexForMap, const mrpt::obs::CSensoryFrame &observation, const mrpt::poses::CPose3D &x) const =0 |
Evaluate the observation likelihood for one particle at a given location. More... | |
Protected Member Functions | |
The generic PF implementations for localization & SLAM. | |
template<class BINTYPE > | |
void | PF_SLAM_implementation_pfAuxiliaryPFOptimal (const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options) |
A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation), common to both localization and mapping. More... | |
template<class BINTYPE > | |
void | PF_SLAM_implementation_pfAuxiliaryPFStandard (const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options) |
A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal), common to both localization and mapping. More... | |
template<class BINTYPE > | |
void | PF_SLAM_implementation_pfStandardProposal (const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options) |
A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter), common to both localization and mapping. More... | |
Private Member Functions | |
template<class BINTYPE > | |
void | PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal (const mrpt::obs::CActionCollection *actions, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const TKLDParams &KLD_options, const bool USE_OPTIMAL_SAMPLING) |
The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPLING. More... | |
template<class BINTYPE > | |
void | PF_SLAM_aux_perform_one_rejection_sampling_step (const bool USE_OPTIMAL_SAMPLING, const bool doResample, const double maxMeanLik, size_t k, const mrpt::obs::CSensoryFrame *sf, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, mrpt::poses::CPose3D &out_newPose, double &out_newParticleLogWeight) |
|
inline |
Definition at line 43 of file PF_implementations_data.h.
|
pure virtual |
Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty).
Implemented in mrpt::maps::CMultiMetricMapPDF, mrpt::slam::CMonteCarloLocalization2D, and mrpt::slam::CMonteCarloLocalization3D.
|
private |
Definition at line 834 of file PF_implementations.h.
References mrpt::poses::CPose3D::composeFrom(), mrpt::random::CRandomGenerator::drawUniform32bit(), mrpt::bayes::CParticleFilter::TParticleFilterOptions::max_loglikelihood_dyn_range, mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE, mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor, mrpt::random::randomGenerator, and mrpt::bayes::CParticleFilter::TParticleFilterOptions::verbose.
|
pure virtual |
Evaluate the observation likelihood for one particle at a given location.
Implemented in mrpt::maps::CMultiMetricMapPDF, mrpt::slam::CMonteCarloLocalization2D, and mrpt::slam::CMonteCarloLocalization3D.
|
pure virtual |
|
inlinevirtual |
Definition at line 226 of file PF_implementations_data.h.
References MRPT_UNUSED_PARAM.
|
protected |
A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation), common to both localization and mapping.
BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
This method implements optimal sampling with a rejection sampling-based approximation of the true posterior. For details, see the papers:
J.L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal, "An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization," in Proc. IEEE International Conference on Robotics and Automation (ICRA'08), 2008, pp. 461-466.
BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
This method implements optimal sampling with a rejection sampling-based approximation of the true posterior. For details, see the papers:
J.-L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal, "An Optimal Filtering Algorithm for Non-Parametric Observation Models in Robot Localization," in Proc. IEEE International Conference on Robotics and Automation (ICRA'08), 2008, pp. 461466.
Definition at line 130 of file PF_implementations.h.
|
protected |
A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal), common to both localization and mapping.
BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
This method is described in the paper: Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary Particle Filters". Journal of the American Statistical Association 94 (446): 590-591. doi:10.2307/2670179.
Definition at line 315 of file PF_implementations.h.
|
private |
The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPLING.
Definition at line 495 of file PF_implementations.h.
References mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize, ASSERT_, mrpt::bayes::CParticleFilter::TParticleFilterOptions::BETA, mrpt::math::chi2inv(), mrpt::math::distance(), mrpt::random::CRandomGenerator::drawUniform32bit(), INVALID_LIKELIHOOD_VALUE, mrpt::slam::TKLDParams::KLD_delta, mrpt::slam::TKLDParams::KLD_epsilon, mrpt::slam::TKLDParams::KLD_maxSampleSize, mrpt::slam::TKLDParams::KLD_minSampleSize, mrpt::slam::TKLDParams::KLD_minSamplesPerBin, mrpt::math::maximum(), mrpt::math::mean(), mrpt::math::minimum(), MRPT_END, MRPT_START, mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE, mrpt::random::random_generator_for_STL(), mrpt::random::randomGenerator, mrpt::utils::round(), and mrpt::bayes::CParticleFilter::TParticleFilterOptions::verbose.
|
protected |
A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter), common to both localization and mapping.
Definition at line 148 of file PF_implementations.h.
References mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize, mrpt::obs::CActionCollection::getActionByClass(), mrpt::obs::CActionCollection::getBestMovementEstimation(), mrpt::poses::CPose3DPDFGaussian::getMean(), mrpt::slam::TKLDParams::KLD_delta, mrpt::slam::TKLDParams::KLD_epsilon, mrpt::slam::TKLDParams::KLD_minSampleSize, MRPT_START, mrpt::obs::CActionRobotMovement3D::poseChange, and THROW_EXCEPTION.
|
inlinevirtual |
This is the default algorithm to efficiently replace one old set of samples by another new set.
The method uses pointers to make fast copies the first time each particle is duplicated, then makes real copies for the next ones.
Note that more efficient specializations might exist for specific particle data structs.
Definition at line 161 of file PF_implementations_data.h.
References mrpt::utils::delete_safe(), and mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_custom_update_particle_with_new_pose().
|
inlinevirtual |
Make a specialization if needed, eg.
in the first step in SLAM.
Reimplemented in mrpt::maps::CMultiMetricMapPDF.
Definition at line 235 of file PF_implementations_data.h.
|
staticprotected |
Definition at line 330 of file PF_implementations.h.
References ASSERT_, mrpt::math::averageLogLikelihood(), MRPT_CHECK_NORMAL_NUMBER, MRPT_END, MRPT_START, MRPT_UNUSED_PARAM, mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples, and mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE.
|
staticprotected |
Compute w[i]*p(z_t | mu_t^i), with mu_t^i being the mean of the new robot pose.
action | MUST be a "const CPose3D*" |
observation | MUST be a "const CSensoryFrame*" |
Definition at line 404 of file PF_implementations.h.
References ASSERT_, mrpt::math::averageLogLikelihood(), mrpt::poses::CPose3D::composeFrom(), MRPT_CHECK_NORMAL_NUMBER, MRPT_END, MRPT_START, mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples, mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE, and mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterStandard_FirstStageWeightsMonteCarlo.
|
protected |
Definition at line 52 of file PF_implementations_data.h.
|
protected |
Definition at line 53 of file PF_implementations_data.h.
|
protected |
Definition at line 54 of file PF_implementations_data.h.
|
protected |
Definition at line 55 of file PF_implementations_data.h.
|
protected |
Used in al PF implementations.
Definition at line 57 of file PF_implementations_data.h.
|
mutableprotected |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 58 of file PF_implementations_data.h.
|
mutableprotected |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 61 of file PF_implementations_data.h.
|
mutableprotected |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 60 of file PF_implementations_data.h.
|
protected |
Definition at line 62 of file PF_implementations_data.h.
|
mutableprotected |
Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
Definition at line 59 of file PF_implementations_data.h.
Page generated by Doxygen 1.9.4 for MRPT 1.4.0 SVN: at Sun Aug 14 11:34:44 UTC 2022 |