9 #ifndef PF_implementations_data_H
10 #define PF_implementations_data_H
28 template <
class PARTICLETYPE,
class BINTYPE>
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);
This virtual class defines the interface that any particles based PDF class must implement in order t...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
Declares a class for storing a collection of robot actions.
Represents a probabilistic 2D movement of the robot mobile base.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
A set of common data shared by PF implementations for both SLAM and localization.
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).
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)
bool m_accumRobotMovement2DIsValid
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.
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...
mrpt::poses::CPose3DPDFGaussian m_accumRobotMovement3D
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...
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...
mrpt::poses::CPoseRandomSampler m_movementDrawer
Used in al PF implementations.
std::vector< bool > m_pfAuxiliaryPFOptimal_maxLikMovementDrawHasBeenUsed
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
mrpt::obs::CActionRobotMovement2D m_accumRobotMovement2D
virtual bool PF_SLAM_implementation_skipRobotMovement() const
Make a specialization if needed, eg.
std::vector< mrpt::math::TPose3D > m_pfAuxiliaryPFOptimal_maxLikDrawnMovement
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
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)
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,...
mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
bool m_accumRobotMovement3DIsValid
virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(PARTICLE_TYPE *particleData, const mrpt::math::TPose3D &newPose) const =0
virtual bool PF_SLAM_implementation_doWeHaveValidObservations(const typename mrpt::bayes::CParticleFilterData< PARTICLE_TYPE >::CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const
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.
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.
Option set for KLD algorithm.
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,...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void KLF_loadBinFromParticle(BINTYPE &outBin, const TKLDParams &opts, const PARTICLETYPE *currentParticleValue=NULL, const mrpt::math::TPose3D *newPoseToBeInserted=NULL)
void delete_safe(T *&ptr)
Calls "delete" to free an object only if the pointer is not NULL, then set the pointer to NULL.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The configuration of a particle filter.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).