Main MRPT website > C++ reference for MRPT 1.4.0
CParticleFilterCapable.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 CPARTICLEFILTERCAPABLE_H
10#define CPARTICLEFILTERCAPABLE_H
11
14
15namespace mrpt
16{
17namespace bayes
18{
19 #define INVALID_LIKELIHOOD_VALUE (-1e300) // An invalid log-likelihood value, used to signal non-initialized likelihood variables.
20
21 /** This virtual class defines the interface that any particles based PDF class must implement in order to be executed by a mrpt::bayes::CParticleFilter.
22 *
23 * See the <a href="http://www.mrpt.org/Particle_Filter_Tutorial" >Particle Filter tutorial</a> explaining how to use the particle filter-related classes.
24 * \sa CParticleFilter, CParticleFilterData
25 * \ingroup mrpt_base_grp
26 */
28 {
29 friend class CParticleFilter;
30
31 private:
33
34 public:
35
36 CParticleFilterCapable() : m_fastDrawAuxiliary()
37 { }
38
39
40 /** Virtual destructor
41 */
43 {
44 }
45
46 /** A callback function type for evaluating the probability of m_particles of being selected, used in "fastDrawSample".
47 * The default evaluator function "defaultEvaluator" simply returns the particle weight.
48 * \param index This is the index of the particle its probability is being computed.
49 * \param action The value of this is the parameter passed to "prepareFastDrawSample"
50 * \param observation The value of this is the parameter passed to "prepareFastDrawSample"
51 * The action and the observation are declared as "void*" for a greater flexibility.
52 * \sa prepareFastDrawSample
53 */
54 typedef double ( *TParticleProbabilityEvaluator) (
56 const CParticleFilterCapable *obj,
57 size_t index,
58 const void * action,
59 const void * observation );
60
61 /** The default evaluator function, which simply returns the particle weight.
62 * The action and the observation are declared as "void*" for a greater flexibility.
63 * \sa prepareFastDrawSample
64 */
65 static double defaultEvaluator(
67 const CParticleFilterCapable *obj,
68 size_t index,
69 const void * action,
70 const void * observation )
71 {
72 MRPT_UNUSED_PARAM(PF_options); MRPT_UNUSED_PARAM(action); MRPT_UNUSED_PARAM(observation);
73 return obj->getW(index);
74 }
75
76 /** Prepares data structures for calling fastDrawSample method next.
77 * This method must be called once before using "fastDrawSample" (calling this more than once has no effect, but it takes time for nothing!)
78 * The behavior depends on the configuration of the PF (see CParticleFilter::TParticleFilterOptions):
79 * - <b>DYNAMIC SAMPLE SIZE=NO</b>: In this case this method fills out an internal array (m_fastDrawAuxiliary.alreadyDrawnIndexes) with
80 * the random indexes generated according to the selected resample scheme in TParticleFilterOptions. Those indexes are
81 * read sequentially by subsequent calls to fastDrawSample.
82 * - <b>DYNAMIC SAMPLE SIZE=YES</b>: Then:
83 * - If TParticleFilterOptions.resamplingMethod = prMultinomial, the internal buffers will be filled out (m_fastDrawAuxiliary.CDF, CDF_indexes & PDF) and
84 * then fastDrawSample can be called an arbitrary number of times to generate random indexes.
85 * - For the rest of resampling algorithms, an exception will be raised since they are not appropriate for a dynamic (unknown in advance) number of particles.
86 *
87 * The function pointed by "partEvaluator" should take into account the particle filter algorithm selected in "m_PFAlgorithm".
88 * If called without arguments (defaultEvaluator), the default behavior is to draw samples with a probability proportional to their current weights.
89 * The action and the observation are declared as "void*" for a greater flexibility.
90 * For a more detailed information see the <a href="http://www.mrpt.org/Particle_Filters" >Particle Filter tutorial</a>.
91 * Custom supplied "partEvaluator" functions must take into account the previous particle weight, i.e. multiplying the current observation likelihood by the weights.
92 * \sa fastDrawSample
93 */
96 TParticleProbabilityEvaluator partEvaluator = defaultEvaluator,
97 const void * action = NULL,
98 const void * observation = NULL
99 ) const;
100
101 /** Draws a random sample from the particle filter, in such a way that each particle has a probability proportional to its weight (in the standard PF algorithm).
102 * This method can be used to generate a variable number of m_particles when resampling: to vary the number of m_particles in the filter.
103 * See prepareFastDrawSample for more information, or the <a href="http://www.mrpt.org/Particle_Filters" >Particle Filter tutorial</a>.
104 *
105 * NOTES:
106 * - You MUST call "prepareFastDrawSample" ONCE before calling this method. That method must be called after modifying the particle filter (executing one step, resampling, etc...)
107 * - This method returns ONE index for the selected ("drawn") particle, in the range [0,M-1]
108 * - You do not need to call "normalizeWeights" before calling this.
109 * \sa prepareFastDrawSample
110 */
112
113 /** Access to i'th particle (logarithm) weight, where first one is index 0.
114 */
115 virtual double getW(size_t i) const = 0;
116
117 /** Modifies i'th particle (logarithm) weight, where first one is index 0.
118 */
119 virtual void setW(size_t i, double w) = 0;
120
121 /** Get the m_particles count.
122 */
123 virtual size_t particlesCount() const = 0;
124
125 /** Performs the prediction stage of the Particle Filter.
126 * This method simply selects the appropiate protected method according to the particle filter algorithm to run.
127 * \sa prediction_and_update_pfStandardProposal,prediction_and_update_pfAuxiliaryPFStandard,prediction_and_update_pfOptimalProposal,prediction_and_update_pfAuxiliaryPFOptimal
128 */
130 const mrpt::obs::CActionCollection * action,
131 const mrpt::obs::CSensoryFrame * observation,
133 );
134
135 /** Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly.
136 * \param indx The indices of current m_particles to be saved as the new m_particles set.
137 */
138 virtual void performSubstitution( const std::vector<size_t> &indx) = 0;
139
140 /** Normalize the (logarithmic) weights, such as the maximum weight is zero.
141 * \param out_max_log_w If provided, will return with the maximum log_w before normalizing, such as new_weights = old_weights - max_log_w.
142 * \return The max/min ratio of weights ("dynamic range")
143 */
144 virtual double normalizeWeights( double *out_max_log_w = NULL ) =0;
145
146 /** Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
147 * Note that you do NOT need to normalize the weights before calling this.
148 */
149 virtual double ESS() const = 0;
150
151 /** Performs a resample of the m_particles, using the method selected in the constructor.
152 * After computing the surviving samples, this method internally calls "performSubstitution" to actually perform the particle replacement.
153 * This method is called automatically by CParticleFilter::execute, andshould not be invoked manually normally.
154 * To just obtaining the sequence of resampled indexes from a sequence of weights, use "resample"
155 * \param[in] out_particle_count The desired number of output particles after resampling; 0 means don't modify the current number.
156 * \sa resample
157 */
158 void performResampling( const bayes::CParticleFilter::TParticleFilterOptions &PF_options,size_t out_particle_count = 0 );
159
160 /** A static method to perform the computation of the samples resulting from resampling a given set of particles, given their logarithmic weights, and a resampling method.
161 * It returns the sequence of indexes from the resampling. The number of output samples is the same than the input population.
162 * This generic method just computes these indexes, to actually perform a resampling in a particle filter object, call performResampling
163 * \param[in] out_particle_count The desired number of output particles after resampling; 0 means don't modify the current number.
164 * \sa performResampling
165 */
166 static void computeResampling(
168 const std::vector<double> &in_logWeights,
169 std::vector<size_t> &out_indexes,
170 size_t out_particle_count = 0
171 );
172
173 /** A static method to compute the linear, normalized (the sum the unity) weights from log-weights.
174 * \sa performResampling
175 */
176 static void log2linearWeights(
177 const std::vector<double> &in_logWeights,
178 std::vector<double> &out_linWeights );
179
180
181 protected:
182 /** Performs the particle filter prediction/update stages for the algorithm "pfStandardProposal" (if not implemented in heritated class, it will raise a 'non-implemented' exception).
183 * \sa prediction_and_update
184 */
186 const mrpt::obs::CActionCollection * action,
187 const mrpt::obs::CSensoryFrame * observation,
189 /** Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFStandard" (if not implemented in heritated class, it will raise a 'non-implemented' exception).
190 * \sa prediction_and_update
191 */
193 const mrpt::obs::CActionCollection * action,
194 const mrpt::obs::CSensoryFrame * observation,
196 /** Performs the particle filter prediction/update stages for the algorithm "pfOptimalProposal" (if not implemented in heritated class, it will raise a 'non-implemented' exception).
197 * \sa prediction_and_update
198 */
200 const mrpt::obs::CActionCollection * action,
201 const mrpt::obs::CSensoryFrame * observation,
203 /** Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFOptimal" (if not implemented in heritated class, it will raise a 'non-implemented' exception).
204 * \sa prediction_and_update
205 */
207 const mrpt::obs::CActionCollection * action,
208 const mrpt::obs::CSensoryFrame * observation,
210
211 /** Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for more information
212 */
214 {
216 CDF(),
217 CDF_indexes(),
218 PDF(),
219 alreadyDrawnIndexes(),
220 alreadyDrawnNextOne(0)
221 { }
222
223 std::vector<double> CDF;
225 std::vector<double> PDF;
226
229 };
230
231 /** Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for more information
232 */
234
235 }; // End of class def.
236
237 } // end namespace
238} // end namespace
239#endif
This virtual class defines the interface that any particles based PDF class must implement in order t...
TFastDrawAuxVars m_fastDrawAuxiliary
Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for more information.
virtual void prediction_and_update_pfStandardProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the particle filter prediction/update stages for the algorithm "pfStandardProposal" (if not ...
virtual void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFOptimal" (if no...
virtual void prediction_and_update_pfAuxiliaryPFStandard(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFStandard" (if n...
virtual double ESS() const =0
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
virtual double normalizeWeights(double *out_max_log_w=NULL)=0
Normalize the (logarithmic) weights, such as the maximum weight is zero.
static void log2linearWeights(const std::vector< double > &in_logWeights, std::vector< double > &out_linWeights)
A static method to compute the linear, normalized (the sum the unity) weights from log-weights.
virtual double getW(size_t i) const =0
Access to i'th particle (logarithm) weight, where first one is index 0.
virtual size_t particlesCount() const =0
Get the m_particles count.
virtual ~CParticleFilterCapable()
Virtual destructor.
void prediction_and_update(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the prediction stage of the Particle Filter.
static void computeResampling(CParticleFilter::TParticleResamplingAlgorithm method, const std::vector< double > &in_logWeights, std::vector< size_t > &out_indexes, size_t out_particle_count=0)
A static method to perform the computation of the samples resulting from resampling a given set of pa...
static const unsigned PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS
virtual void performSubstitution(const std::vector< size_t > &indx)=0
Performs the substitution for internal use of resample in particle filter algorithm,...
void performResampling(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, size_t out_particle_count=0)
Performs a resample of the m_particles, using the method selected in the constructor.
void prepareFastDrawSample(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, TParticleProbabilityEvaluator partEvaluator=defaultEvaluator, const void *action=NULL, const void *observation=NULL) const
Prepares data structures for calling fastDrawSample method next.
static double defaultEvaluator(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
The default evaluator function, which simply returns the particle weight.
size_t fastDrawSample(const bayes::CParticleFilter::TParticleFilterOptions &PF_options) const
Draws a random sample from the particle filter, in such a way that each particle has a probability pr...
virtual void prediction_and_update_pfOptimalProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the particle filter prediction/update stages for the algorithm "pfOptimalProposal" (if not i...
virtual void setW(size_t i, double w)=0
Modifies i'th particle (logarithm) weight, where first one is index 0.
This class acts as a common interface to the different interfaces (see CParticleFilter::TParticleFilt...
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
Declares a class for storing a collection of robot actions.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
std::vector< uint32_t > vector_uint
Definition: types_simple.h:28
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:290
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The configuration of a particle filter.
Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for more information.



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Fri Jan 20 00:13:14 UTC 2023