Main MRPT website > C++ reference for MRPT 1.4.0
PF_implementations_data.h
Go to the documentation of this file.
1/* +---------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | http://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6 | See: http://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See details in http://www.mrpt.org/License |
8 +---------------------------------------------------------------------------+ */
9#ifndef PF_implementations_data_H
10#define PF_implementations_data_H
11
16#include <mrpt/poses/CPose3D.h>
20
22
23namespace mrpt
24{
25 namespace slam
26 {
27 // Frwd decl:
28 template <class PARTICLETYPE, class BINTYPE>
30 BINTYPE &outBin,
31 const TKLDParams &opts,
32 const PARTICLETYPE *currentParticleValue = NULL,
33 const mrpt::math::TPose3D *newPoseToBeInserted = NULL );
34
35
36 /** A set of common data shared by PF implementations for both SLAM and localization
37 * \ingroup mrpt_slam_grp
38 */
39 template <class PARTICLE_TYPE, class MYSELF>
41 {
42 public:
46 {
47 }
48 protected:
49 /** \name Data members and methods used by generic PF implementations
50 @{ */
51
56
57 mrpt::poses::CPoseRandomSampler m_movementDrawer; //!< Used in al PF implementations. \sa PF_SLAM_implementation_gatherActionsCheckBothActObs
58 mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
59 mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFStandard_estimatedProb; //!< Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
60 mutable mrpt::math::CVectorDouble m_pfAuxiliaryPFOptimal_maxLikelihood; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
61 mutable std::vector<mrpt::math::TPose3D> m_pfAuxiliaryPFOptimal_maxLikDrawnMovement; //!< Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
63
64 /** Compute w[i]*p(z_t | mu_t^i), with mu_t^i being
65 * the mean of the new robot pose
66 *
67 * \param action MUST be a "const CPose3D*"
68 * \param observation MUST be a "const CSensoryFrame*"
69 */
70 template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
74 size_t index,
75 const void * action,
76 const void * observation );
77
78 template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
82 size_t index,
83 const void *action,
84 const void *observation );
85
86 /** @} */
87
88 /** \name The generic PF implementations for localization & SLAM.
89 @{ */
90
91 /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFOptimal" (optimal sampling with rejection sampling approximation),
92 * common to both localization and mapping.
93 *
94 * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
95 *
96 * This method implements optimal sampling with a rejection sampling-based approximation of the true posterior.
97 * For details, see the papers:
98 *
99 * J.L. Blanco, J. Gonzalez, and J.-A. Fernandez-Madrigal,
100 * "An Optimal Filtering Algorithm for Non-Parametric Observation Models in
101 * Robot Localization," in Proc. IEEE International Conference on Robotics
102 * and Automation (ICRA'08), 2008, pp. 461-466.
103 */
104 template <class BINTYPE>
106 const mrpt::obs::CActionCollection * actions,
107 const mrpt::obs::CSensoryFrame * sf,
109 const TKLDParams &KLD_options);
110
111 /** A generic implementation of the PF method "prediction_and_update_pfAuxiliaryPFStandard" (Auxiliary particle filter with the standard proposal),
112 * common to both localization and mapping.
113 *
114 * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
115 *
116 * This method is described in the paper:
117 * Pitt, M.K.; Shephard, N. (1999). "Filtering Via Simulation: Auxiliary Particle Filters".
118 * Journal of the American Statistical Association 94 (446): 590-591. doi:10.2307/2670179.
119 *
120 */
121 template <class BINTYPE>
123 const mrpt::obs::CActionCollection * actions,
124 const mrpt::obs::CSensoryFrame * sf,
126 const TKLDParams &KLD_options);
127
128
129 /** A generic implementation of the PF method "pfStandardProposal" (standard proposal distribution, that is, a simple SIS particle filter),
130 * common to both localization and mapping.
131 *
132 * - BINTYPE: TPoseBin or whatever to discretize the sample space for KLD-sampling.
133 */
134 template <class BINTYPE>
136 const mrpt::obs::CActionCollection * actions,
137 const mrpt::obs::CSensoryFrame * sf,
139 const TKLDParams &KLD_options);
140
141 /** @} */
142
143
144 public:
145 /** \name Virtual methods that the PF_implementations assume exist.
146 @{ */
147
148 /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
149 virtual const mrpt::math::TPose3D * getLastPose(const size_t i) const = 0;
150
152 PARTICLE_TYPE *particleData,
153 const mrpt::math::TPose3D &newPose) const = 0;
154
155 /** This is the default algorithm to efficiently replace one old set of samples by another new set.
156 * The method uses pointers to make fast copies the first time each particle is duplicated, then
157 * makes real copies for the next ones.
158 *
159 * Note that more efficient specializations might exist for specific particle data structs.
160 */
163 const std::vector<mrpt::math::TPose3D> &newParticles,
164 const std::vector<double> &newParticlesWeight,
165 const std::vector<size_t> &newParticlesDerivedFromIdx ) const
166 {
167 // ---------------------------------------------------------------------------------
168 // Substitute old by new particle set:
169 // Old are in "m_particles"
170 // New are in "newParticles", "newParticlesWeight","newParticlesDerivedFromIdx"
171 // ---------------------------------------------------------------------------------
172 const size_t N = newParticles.size();
173 typename MYSELF::CParticleList newParticlesArray(N);
174
175 // For efficiency, just copy the "CParticleData" from the old particle into the
176 // new one, but this can be done only once:
177 std::vector<bool> oldParticleAlreadyCopied(old_particles.size(),false);
178
179 size_t i;
180 typename MYSELF::CParticleList::iterator newPartIt;
181 for (newPartIt=newParticlesArray.begin(),i=0;newPartIt!=newParticlesArray.end();++newPartIt,++i)
182 {
183 // The weight:
184 newPartIt->log_w = newParticlesWeight[i];
185
186 // The data (CParticleData):
187 PARTICLE_TYPE *newPartData;
188 if (!oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]])
189 {
190 // The first copy of this old particle:
191 newPartData = old_particles[ newParticlesDerivedFromIdx[i] ].d;
192 oldParticleAlreadyCopied[newParticlesDerivedFromIdx[i]] = true;
193 }
194 else
195 {
196 // Make a copy:
197 newPartData = new PARTICLE_TYPE( *old_particles[ newParticlesDerivedFromIdx[i] ].d );
198 }
199
200 newPartIt->d = newPartData;
201 } // end for "newPartIt"
202
203 // Now add the new robot pose to the paths:
204 // (this MUST be done after the above loop, separately):
205 // Update the particle with the new pose: this part is caller-dependant and must be implemented there:
206 for (newPartIt=newParticlesArray.begin(),i=0;i<N;++newPartIt,++i)
208
209 // Free those old m_particles not being copied into the new ones:
210 for (size_t i=0;i<old_particles.size();i++)
211 if (!oldParticleAlreadyCopied[i])
212 mrpt::utils::delete_safe( old_particles[ i ].d );
213
214 // Copy into "m_particles"
215 old_particles.resize( newParticlesArray.size() );
216 typename MYSELF::CParticleList::iterator trgPartIt;
217 for (newPartIt=newParticlesArray.begin(),trgPartIt=old_particles.begin(); newPartIt!=newParticlesArray.end(); ++newPartIt, ++trgPartIt )
218 {
219 trgPartIt->log_w = newPartIt->log_w;
220 trgPartIt->d = newPartIt->d;
221 }
222 } // end of PF_SLAM_implementation_replaceByNewParticleSet
223
224
225
228 const mrpt::obs::CSensoryFrame *sf) const
229 {
230 MRPT_UNUSED_PARAM(particles); MRPT_UNUSED_PARAM(sf);
231 return true; // By default, always process the SFs.
232 }
233
234 /** Make a specialization if needed, eg. in the first step in SLAM. */
236 {
237 return false; // By default, always allow the robot to move!
238 }
239
240 /** Evaluate the observation likelihood for one particle at a given location */
243 const size_t particleIndexForMap,
244 const mrpt::obs::CSensoryFrame &observation,
245 const mrpt::poses::CPose3D &x ) const = 0;
246
247 /** @} */
248
249
250 /** Auxiliary method called by PF implementations: return true if we have both action & observation,
251 * otherwise, return false AND accumulate the odometry so when we have an observation we didn't lose a thing.
252 * On return=true, the "m_movementDrawer" member is loaded and ready to draw samples of the increment of pose since last step.
253 * This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.
254 */
255 template <class BINTYPE> // Template arg. actually not used, just to allow giving the definition in another file later on
257 const mrpt::obs::CActionCollection * actions,
258 const mrpt::obs::CSensoryFrame * sf );
259
260 private:
261 /** The shared implementation body of two PF methods: APF and Optimal-APF, depending on USE_OPTIMAL_SAMPLING */
262 template <class BINTYPE>
264 const mrpt::obs::CActionCollection * actions,
265 const mrpt::obs::CSensoryFrame * sf,
267 const TKLDParams &KLD_options,
268 const bool USE_OPTIMAL_SAMPLING );
269
270 template <class BINTYPE>
272 const bool USE_OPTIMAL_SAMPLING,
273 const bool doResample,
274 const double maxMeanLik,
275 size_t k, // The particle from the old set "m_particles[]"
276 const mrpt::obs::CSensoryFrame * sf,
278 mrpt::poses::CPose3D & out_newPose,
279 double & out_newParticleLogWeight);
280
281
282 }; // end PF_implementation
283 }
284}
285
286#endif
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.
Definition: types_math.h:65
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).
Definition: CPose3D.h:73
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.
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)
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,...
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::math::CVectorDouble m_pfAuxiliaryPFOptimal_estimatedProb
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
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.
Definition: TKLDParams.h:23
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.
Definition: mrpt_macros.h:290
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.
Definition: bits.h:165
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).



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Thu Mar 23 03:22:58 UTC 2023