A set of common data shared by PF implementations for both SLAM and localization.
Definition at line 63 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 CActionCollection *actions, const 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. | |
Virtual methods that the PF_implementations assume exist. | |
virtual const 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). | |
virtual void | PF_SLAM_implementation_custom_update_particle_with_new_pose (PARTICLE_TYPE *particleData, const TPose3D &newPose) const =0 |
virtual void | PF_SLAM_implementation_replaceByNewParticleSet (typename CParticleFilterData< PARTICLE_TYPE >::CParticleList &old_particles, const vector< TPose3D > &newParticles, const vector_double &newParticlesWeight, const vector< size_t > &newParticlesDerivedFromIdx) const |
This is the default algorithm to efficiently replace one old set of samples by another new set. | |
virtual bool | PF_SLAM_implementation_doWeHaveValidObservations (const typename CParticleFilterData< PARTICLE_TYPE >::CParticleList &particles, const CSensoryFrame *sf) const |
virtual bool | PF_SLAM_implementation_skipRobotMovement () const |
Make a specialization if needed, eg. | |
virtual double | PF_SLAM_computeObservationLikelihoodForParticle (const CParticleFilter::TParticleFilterOptions &PF_options, const size_t particleIndexForMap, const CSensoryFrame &observation, const CPose3D &x) const =0 |
Evaluate the observation likelihood for one particle at a given location. | |
Protected Member Functions | |
The generic PF implementations for localization & SLAM. | |
template<class BINTYPE > | |
void | PF_SLAM_implementation_pfAuxiliaryPFOptimal (const CActionCollection *actions, const CSensoryFrame *sf, const 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. | |
template<class BINTYPE > | |
void | PF_SLAM_implementation_pfAuxiliaryPFStandard (const CActionCollection *actions, const CSensoryFrame *sf, const 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. | |
template<class BINTYPE > | |
void | PF_SLAM_implementation_pfStandardProposal (const CActionCollection *actions, const CSensoryFrame *sf, const 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. | |
Private Member Functions | |
template<class BINTYPE > | |
void | PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal (const CActionCollection *actions, const CSensoryFrame *sf, const 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. | |
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 CSensoryFrame *sf, const CParticleFilter::TParticleFilterOptions &PF_options, CPose3D &out_newPose, double &out_newParticleLogWeight) |
Data members and methods used by generic PF implementations | |
CActionRobotMovement2D | m_accumRobotMovement2D |
bool | m_accumRobotMovement2DIsValid |
CPose3DPDFGaussian | m_accumRobotMovement3D |
bool | m_accumRobotMovement3DIsValid |
CPoseRandomSampler | m_movementDrawer |
Used in al PF implementations. | |
vector_double | m_pfAuxiliaryPFOptimal_estimatedProb |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
vector_double | m_pfAuxiliaryPFStandard_estimatedProb |
Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm. | |
vector_double | m_pfAuxiliaryPFOptimal_maxLikelihood |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
std::vector< TPose3D > | m_pfAuxiliaryPFOptimal_maxLikDrawnMovement |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
std::vector< bool > | m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed |
template<class BINTYPE > | |
static double | PF_SLAM_particlesEvaluator_AuxPFStandard (const CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t index, const void *action, const void *observation) |
Compute w[i]·p(z_t | mu_t^i), with mu_t^i being the mean of the new robot pose. | |
template<class BINTYPE > | |
static double | PF_SLAM_particlesEvaluator_AuxPFOptimal (const CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t index, const void *action, const void *observation) |
mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_implementation | ( | ) | [inline] |
Definition at line 66 of file PF_implementations_data.h.
virtual const TPose3D* mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::getLastPose | ( | const size_t | i ) | const [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::slam::CMonteCarloLocalization2D, mrpt::slam::CMonteCarloLocalization3D, and mrpt::slam::CMultiMetricMapPDF.
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_aux_perform_one_rejection_sampling_step | ( | const bool | USE_OPTIMAL_SAMPLING, |
const bool | doResample, | ||
const double | maxMeanLik, | ||
size_t | k, | ||
const CSensoryFrame * | sf, | ||
const CParticleFilter::TParticleFilterOptions & | PF_options, | ||
CPose3D & | out_newPose, | ||
double & | out_newParticleLogWeight | ||
) | [private] |
Definition at line 858 of file PF_implementations.h.
References mrpt::poses::CPose3D::composeFrom(), mrpt::random::CRandomGenerator::drawUniform(), mrpt::random::CRandomGenerator::drawUniform32bit(), exp(), 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.
virtual double mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_computeObservationLikelihoodForParticle | ( | const CParticleFilter::TParticleFilterOptions & | PF_options, |
const size_t | particleIndexForMap, | ||
const CSensoryFrame & | observation, | ||
const CPose3D & | x | ||
) | const [pure virtual] |
Evaluate the observation likelihood for one particle at a given location.
Implemented in mrpt::slam::CMonteCarloLocalization2D, mrpt::slam::CMonteCarloLocalization3D, and mrpt::slam::CMultiMetricMapPDF.
virtual void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_custom_update_particle_with_new_pose | ( | PARTICLE_TYPE * | particleData, |
const TPose3D & | newPose | ||
) | const [pure virtual] |
virtual bool mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_doWeHaveValidObservations | ( | const typename CParticleFilterData< PARTICLE_TYPE >::CParticleList & | particles, |
const CSensoryFrame * | sf | ||
) | const [inline, virtual] |
Definition at line 257 of file PF_implementations_data.h.
bool mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_gatherActionsCheckBothActObs | ( | const CActionCollection * | actions, |
const 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.
On return=true, the "m_movementDrawer" member is loaded and ready to draw samples of the increment of pose since last step. This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.
Definition at line 69 of file PF_implementations.h.
References mrpt::slam::CActionRobotMovement2D::computeFromOdometry(), mrpt::slam::CActionCollection::getActionByClass(), mrpt::slam::CActionCollection::getBestMovementEstimation(), mrpt::slam::CActionRobotMovement2D::poseChange, stlplus::smart_ptr_base< T, C, COUNTER >::present(), and THROW_EXCEPTION.
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_pfAuxiliaryPFOptimal | ( | const CActionCollection * | actions, |
const CSensoryFrame * | sf, | ||
const CParticleFilter::TParticleFilterOptions & | PF_options, | ||
const TKLDParams & | KLD_options | ||
) | [protected] |
A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation), common to both localization and mapping.
This method implements optimal sampling with a rejection sampling-based approximation of the true posterior. For details, see the papers:
J.-L. Blanco, J. González, and J.-A. Fernández-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.
This method implements optimal sampling with a rejection sampling-based approximation of the true posterior. For details, see the papers:
J.-L. Blanco, J. González, and J.-A. Fernández-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.
Definition at line 153 of file PF_implementations.h.
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_pfAuxiliaryPFStandard | ( | const CActionCollection * | actions, |
const CSensoryFrame * | sf, | ||
const CParticleFilter::TParticleFilterOptions & | PF_options, | ||
const TKLDParams & | KLD_options | ||
) | [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.
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.
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 338 of file PF_implementations.h.
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_pfAuxiliaryPFStandardAndOptimal | ( | const CActionCollection * | actions, |
const CSensoryFrame * | sf, | ||
const CParticleFilter::TParticleFilterOptions & | PF_options, | ||
const TKLDParams & | KLD_options, | ||
const bool | USE_OPTIMAL_SAMPLING | ||
) | [private] |
The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPLING.
Definition at line 517 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, maximum(), 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(), mrpt::math::TPose3D::size(), and mrpt::bayes::CParticleFilter::TParticleFilterOptions::verbose.
void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_pfStandardProposal | ( | const CActionCollection * | actions, |
const CSensoryFrame * | sf, | ||
const CParticleFilter::TParticleFilterOptions & | PF_options, | ||
const TKLDParams & | KLD_options | ||
) | [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 171 of file PF_implementations.h.
References mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize, mrpt::math::chi2inv(), mrpt::slam::CActionCollection::getActionByClass(), mrpt::slam::CActionCollection::getBestMovementEstimation(), mrpt::slam::TKLDParams::KLD_delta, mrpt::slam::TKLDParams::KLD_epsilon, mrpt::slam::TKLDParams::KLD_maxSampleSize, mrpt::slam::TKLDParams::KLD_minSampleSize, MRPT_END, MRPT_START, mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor, stlplus::smart_ptr_base< T, C, COUNTER >::present(), mrpt::utils::round(), and THROW_EXCEPTION.
virtual void mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_replaceByNewParticleSet | ( | typename CParticleFilterData< PARTICLE_TYPE >::CParticleList & | old_particles, |
const vector< TPose3D > & | newParticles, | ||
const vector_double & | newParticlesWeight, | ||
const vector< size_t > & | newParticlesDerivedFromIdx | ||
) | const [inline, virtual] |
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 192 of file PF_implementations_data.h.
virtual bool mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_skipRobotMovement | ( | ) | const [inline, virtual] |
Make a specialization if needed, eg.
in the first step in SLAM.
Reimplemented in mrpt::slam::CMultiMetricMapPDF.
Definition at line 265 of file PF_implementations_data.h.
double mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_particlesEvaluator_AuxPFOptimal | ( | const CParticleFilter::TParticleFilterOptions & | PF_options, |
const CParticleFilterCapable * | obj, | ||
size_t | index, | ||
const void * | action, | ||
const void * | observation | ||
) | [static, protected] |
Definition at line 353 of file PF_implementations.h.
References ASSERT_, mrpt::math::averageLogLikelihood(), MRPT_CHECK_NORMAL_NUMBER, MRPT_END, MRPT_START, mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples, and mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE.
double mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_particlesEvaluator_AuxPFStandard | ( | const CParticleFilter::TParticleFilterOptions & | PF_options, |
const CParticleFilterCapable * | obj, | ||
size_t | index, | ||
const void * | action, | ||
const void * | observation | ||
) | [static, protected] |
Compute w[i]·p(z_t | mu_t^i), with mu_t^i being the mean of the new robot pose.
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 426 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.
CActionRobotMovement2D mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_accumRobotMovement2D [protected] |
Definition at line 83 of file PF_implementations_data.h.
bool mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_accumRobotMovement2DIsValid [protected] |
Definition at line 84 of file PF_implementations_data.h.
CPose3DPDFGaussian mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_accumRobotMovement3D [protected] |
Definition at line 85 of file PF_implementations_data.h.
bool mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_accumRobotMovement3DIsValid [protected] |
Definition at line 86 of file PF_implementations_data.h.
CPoseRandomSampler mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_movementDrawer [protected] |
Used in al PF implementations.
Definition at line 88 of file PF_implementations_data.h.
vector_double mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFOptimal_estimatedProb [mutable, protected] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 89 of file PF_implementations_data.h.
std::vector<TPose3D> mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFOptimal_maxLikDrawnMovement [mutable, protected] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 92 of file PF_implementations_data.h.
vector_double mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFOptimal_maxLikelihood [mutable, protected] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 91 of file PF_implementations_data.h.
std::vector<bool> mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed [protected] |
Definition at line 93 of file PF_implementations_data.h.
vector_double mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::m_pfAuxiliaryPFStandard_estimatedProb [mutable, protected] |
Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
Definition at line 90 of file PF_implementations_data.h.
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |