Go to the documentation of this file.
9 #ifndef PF_implementations_data_H
10 #define PF_implementations_data_H
28 template <
class PARTICLETYPE,
class BINTYPE>
31 const TKLDParams &opts,
32 const PARTICLETYPE *currentParticleValue = NULL,
39 template <
class PARTICLE_TYPE,
class MY
SELF>
70 template <
class BINTYPE>
76 const void * observation );
78 template <
class BINTYPE>
84 const void *observation );
104 template <
class BINTYPE>
121 template <
class BINTYPE>
134 template <
class BINTYPE>
152 PARTICLE_TYPE *particleData,
163 const std::vector<mrpt::math::TPose3D> &newParticles,
164 const std::vector<double> &newParticlesWeight,
165 const std::vector<size_t> &newParticlesDerivedFromIdx )
const
172 const size_t N = newParticles.size();
173 typename MYSELF::CParticleList newParticlesArray(N);
177 std::vector<bool> oldParticleAlreadyCopied(old_particles.size(),
false);
181 for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();++newPartIt,++i)
184 newPartIt->log_w = newParticlesWeight[i];
187 PARTICLE_TYPE *newPartData;
188 if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
191 newPartData = old_particles[ newParticlesDerivedFromIdx[i] ].d;
192 oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] =
true;
197 newPartData =
new PARTICLE_TYPE( *old_particles[ newParticlesDerivedFromIdx[i] ].d );
200 newPartIt->d = newPartData;
206 for (newPartIt=newParticlesArray.begin(),i=0;i<N;++newPartIt,++i)
210 for (
size_t i=0;i<old_particles.size();i++)
211 if (!oldParticleAlreadyCopied[i])
215 old_particles.resize( newParticlesArray.size() );
217 for (newPartIt=newParticlesArray.begin(),trgPartIt=old_particles.begin(); newPartIt!=newParticlesArray.end(); ++newPartIt, ++trgPartIt )
219 trgPartIt->log_w = newPartIt->log_w;
220 trgPartIt->d = newPartIt->d;
243 const size_t particleIndexForMap,
255 template <
class BINTYPE>
262 template <
class BINTYPE>
268 const bool USE_OPTIMAL_SAMPLING );
270 template <
class BINTYPE>
272 const bool USE_OPTIMAL_SAMPLING,
273 const bool doResample,
274 const double maxMeanLik,
279 double & out_newParticleLogWeight);
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.
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
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,...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
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)
bool m_accumRobotMovement3DIsValid
Option set for KLD algorithm.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(PARTICLE_TYPE *particleData, const mrpt::math::TPose3D &newPose) const =0
Declares a class for storing a collection of robot actions.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Represents a probabilistic 2D movement of the robot mobile base.
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL.
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.
static double PF_SLAM_particlesEvaluator_AuxPFOptimal(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
mrpt::poses::CPose3DPDFGaussian m_accumRobotMovement3D
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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...
virtual bool PF_SLAM_implementation_doWeHaveValidObservations(const typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const
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_SAMPL...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
A set of common data shared by PF implementations for both SLAM and localization.
static double PF_SLAM_particlesEvaluator_AuxPFStandard(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const mrpt::bayes::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.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
bool m_accumRobotMovement2DIsValid
void KLF_loadBinFromParticle(BINTYPE &outBin, const TKLDParams &opts, const PARTICLETYPE *currentParticleValue=NULL, const mrpt::math::TPose3D *newPoseToBeInserted=NULL)
mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
std::vector< mrpt::math::TPose3D > m_pfAuxiliaryPFOptimal_maxLikDrawnMovement
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement2D
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,...
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 sampl...
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 pa...
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).
mrpt::poses::CPoseRandomSampler m_movementDrawer
Used in al PF implementations.
The configuration of a particle filter.
std::vector< bool > m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed
virtual bool PF_SLAM_implementation_skipRobotMovement() const
Make a specialization if needed, eg.
Page generated by Doxygen 1.8.16 for MRPT 1.4.0 SVN: at Mon Oct 14 22:32:58 UTC 2019 | | |