Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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 
12 #include <mrpt/utils/utils_defs.h>
14 
15 namespace mrpt
16 {
17 namespace 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) (
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(
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  */
94  void prepareFastDrawSample(
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  */
111  size_t fastDrawSample( const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) const;
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  */
129  void prediction_and_update(
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  */
185  virtual void prediction_and_update_pfStandardProposal(
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  */
192  virtual void prediction_and_update_pfAuxiliaryPFStandard(
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  */
199  virtual void prediction_and_update_pfOptimalProposal(
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  */
206  virtual void prediction_and_update_pfAuxiliaryPFOptimal(
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
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.
std::vector< uint32_t > vector_uint
Definition: types_simple.h:28
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
Declares a class for storing a collection of robot actions.
Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for more information.
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
virtual double getW(size_t i) const =0
Access to i'th particle (logarithm) weight, where first one is index 0.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
virtual ~CParticleFilterCapable()
Virtual destructor.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
GLhandleARB obj
Definition: glew.h:3276
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...
TFastDrawAuxVars m_fastDrawAuxiliary
Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for more information.
GLuint index
Definition: glew.h:1721
The configuration of a particle filter.
static const unsigned PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018