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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019