00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 University of Malaga | 00007 | | 00008 | This software was written by the Machine Perception and Intelligent | 00009 | Robotics Lab, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 00026 | | 00027 +---------------------------------------------------------------------------+ */ 00028 #ifndef CPARTICLEFILTERCAPABLE_H 00029 #define CPARTICLEFILTERCAPABLE_H 00030 00031 #include <mrpt/utils/utils_defs.h> 00032 #include <mrpt/bayes/CParticleFilter.h> 00033 00034 namespace mrpt 00035 { 00036 namespace bayes 00037 { 00038 /** This virtual class defines the interface that any particles based PDF class must implement in order to be executed by a mrpt::bayes::CParticleFilter. 00039 * 00040 * See the <a href="http://babel.isa.uma.es/mrpt/index.php/Particle_Filter_Tutorial">Particle Filter tutorial</a> explaining how to use the particle filter-related classes. 00041 * \sa CParticleFilter, CParticleFilterData 00042 */ 00043 class MRPTDLLIMPEXP CParticleFilterCapable 00044 { 00045 friend class CParticleFilter; 00046 00047 private: 00048 static const unsigned PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS; 00049 00050 public: 00051 00052 CParticleFilterCapable() : m_fastDrawAuxiliary() 00053 { } 00054 00055 00056 /** Virtual destructor 00057 */ 00058 virtual ~CParticleFilterCapable() 00059 { 00060 } 00061 00062 /** A callback function type for evaluating the probability of m_particles of being selected, used in "fastDrawSample". 00063 * The default evaluator function "defaultEvaluator" simply returns the particle weight. 00064 * \param index This is the index of the particle its probability is being computed. 00065 * \param action The value of this is the parameter passed to "prepareFastDrawSample" 00066 * \param observation The value of this is the parameter passed to "prepareFastDrawSample" 00067 * The action and the observation are declared as "void*" for a greater flexibility. 00068 * \sa prepareFastDrawSample 00069 */ 00070 typedef double ( *TParticleProbabilityEvaluator) ( 00071 const bayes::CParticleFilter::TParticleFilterOptions &PF_options, 00072 const CParticleFilterCapable *obj, 00073 size_t index, 00074 const void * action, 00075 const void * observation ); 00076 00077 /** The default evaluator function, which simply returns the particle weight. 00078 * The action and the observation are declared as "void*" for a greater flexibility. 00079 * \sa prepareFastDrawSample 00080 */ 00081 static double defaultEvaluator( 00082 const bayes::CParticleFilter::TParticleFilterOptions &PF_options, 00083 const CParticleFilterCapable *obj, 00084 size_t index, 00085 const void * action, 00086 const void * observation ) 00087 { 00088 MRPT_UNUSED_PARAM(action); MRPT_UNUSED_PARAM(observation); 00089 return obj->getW(index); 00090 } 00091 00092 /** Prepares data structures for calling fastDrawSample method next. 00093 * This method must be called once before using "fastDrawSample" (calling this more than once has no effect, but it takes time for nothing!) 00094 * The behavior depends on the configuration of the PF (see CParticleFilter::TParticleFilterOptions): 00095 * - <b>DYNAMIC SAMPLE SIZE=NO</b>: In this case this method fills out an internal array (m_fastDrawAuxiliary.alreadyDrawnIndexes) with 00096 * the random indexes generated according to the selected resample scheme in TParticleFilterOptions. Those indexes are 00097 * read sequentially by subsequent calls to fastDrawSample. 00098 * - <b>DYNAMIC SAMPLE SIZE=YES</b>: Then: 00099 * - If TParticleFilterOptions.resamplingMethod = prMultinomial, the internal buffers will be filled out (m_fastDrawAuxiliary.CDF, CDF_indexes & PDF) and 00100 * then fastDrawSample can be called an arbitrary number of times to generate random indexes. 00101 * - 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. 00102 * 00103 * The function pointed by "partEvaluator" should take into account the particle filter algorithm selected in "m_PFAlgorithm". 00104 * If called without arguments (defaultEvaluator), the default behavior is to draw samples with a probability proportional to their current weights. 00105 * The action and the observation are declared as "void*" for a greater flexibility. 00106 * For a more detailed information see the <a href="http://babel.isa.uma.es/mrpt/index.php/Particle_Filters">Particle Filter tutorial</a>. 00107 * Custom supplied "partEvaluator" functions must take into account the previous particle weight, i.e. multiplying the current observation likelihood by the weights. 00108 * \sa fastDrawSample 00109 */ 00110 void prepareFastDrawSample( 00111 const bayes::CParticleFilter::TParticleFilterOptions &PF_options, 00112 TParticleProbabilityEvaluator partEvaluator = defaultEvaluator, 00113 const void * action = NULL, 00114 const void * observation = NULL 00115 ) const; 00116 00117 /** 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). 00118 * 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. 00119 * See prepareFastDrawSample for more information, or the <a href="http://babel.isa.uma.es/mrpt/index.php/Particle_Filters">Particle Filter tutorial</a>. 00120 * 00121 * NOTES: 00122 * - You MUST call "prepareFastDrawSample" ONCE before calling this method. That method must be called after modifying the particle filter (executing one step, resampling, etc...) 00123 * - This method returns ONE index for the selected ("drawn") particle, in the range [0,M-1] 00124 * - You do not need to call "normalizeWeights" before calling this. 00125 * \sa prepareFastDrawSample 00126 */ 00127 size_t fastDrawSample( const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) const; 00128 00129 /** Access to i'th particle (logarithm) weight, where first one is index 0. 00130 */ 00131 virtual double getW(size_t i) const = 0; 00132 00133 /** Modifies i'th particle (logarithm) weight, where first one is index 0. 00134 */ 00135 virtual void setW(size_t i, double w) = 0; 00136 00137 /** Get the m_particles count. 00138 */ 00139 virtual size_t particlesCount() const = 0; 00140 00141 /** Performs the prediction stage of the Particle Filter. 00142 * This method simply selects the appropiate protected method according to the particle filter algorithm to run. 00143 * \sa prediction_and_update_pfStandardProposal,prediction_and_update_pfAuxiliaryPFStandard,prediction_and_update_pfOptimalProposal,prediction_and_update_pfAuxiliaryPFOptimal 00144 */ 00145 void prediction_and_update( 00146 const mrpt::slam::CActionCollection * action, 00147 const mrpt::slam::CSensoryFrame * observation, 00148 const bayes::CParticleFilter::TParticleFilterOptions &PF_options 00149 ); 00150 00151 /** Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly. 00152 * \param indx The indices of current m_particles to be saved as the new m_particles set. 00153 */ 00154 virtual void performSubstitution( const std::vector<size_t> &indx) = 0; 00155 00156 /** Normalize the (logarithmic) weights, such as the maximum weight is zero. 00157 * \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. 00158 * \return The max/min ratio of weights ("dynamic range") 00159 */ 00160 virtual double normalizeWeights( double *out_max_log_w = NULL ) =0; 00161 00162 /** Returns the normalized ESS (Estimated Sample Size), in the range [0,1]. 00163 * Note that you do NOT need to normalize the weights before calling this. 00164 */ 00165 virtual double ESS() = 0; 00166 00167 /** Performs a resample of the m_particles, using the method selected in the constructor. 00168 * After computing the surviving samples, this method internally calls "performSubstitution" to actually perform the particle replacement. 00169 * This method is called automatically by CParticleFilter::execute, andshould not be invoked manually normally. 00170 * To just obtaining the sequence of resampled indexes from a sequence of weights, use "resample" 00171 * \sa resample 00172 */ 00173 void performResampling( const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00174 00175 /** 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. 00176 * It returns the sequence of indexes from the resampling. The number of output samples is the same than the input population. 00177 * This generic method just computes these indexes, to actually perform a resampling in a particle filter object, call performResampling 00178 * \sa performResampling 00179 */ 00180 static void computeResampling( 00181 CParticleFilter::TParticleResamplingAlgorithm method, 00182 const std::vector<double> &in_logWeights, 00183 std::vector<size_t> &out_indexes 00184 ); 00185 00186 /** A static method to compute the linear, normalized (the sum the unity) weights from log-weights. 00187 * \sa performResampling 00188 */ 00189 static void log2linearWeights( 00190 const vector_double &in_logWeights, 00191 vector_double &out_linWeights ); 00192 00193 00194 protected: 00195 /** Performs the particle filter prediction/update stages for the algorithm "pfStandardProposal" (if not implemented in heritated class, it will raise a 'non-implemented' exception). 00196 * \sa prediction_and_update 00197 */ 00198 virtual void prediction_and_update_pfStandardProposal( 00199 const mrpt::slam::CActionCollection * action, 00200 const mrpt::slam::CSensoryFrame * observation, 00201 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00202 /** Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFStandard" (if not implemented in heritated class, it will raise a 'non-implemented' exception). 00203 * \sa prediction_and_update 00204 */ 00205 virtual void prediction_and_update_pfAuxiliaryPFStandard( 00206 const mrpt::slam::CActionCollection * action, 00207 const mrpt::slam::CSensoryFrame * observation, 00208 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00209 /** Performs the particle filter prediction/update stages for the algorithm "pfOptimalProposal" (if not implemented in heritated class, it will raise a 'non-implemented' exception). 00210 * \sa prediction_and_update 00211 */ 00212 virtual void prediction_and_update_pfOptimalProposal( 00213 const mrpt::slam::CActionCollection * action, 00214 const mrpt::slam::CSensoryFrame * observation, 00215 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00216 /** Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFOptimal" (if not implemented in heritated class, it will raise a 'non-implemented' exception). 00217 * \sa prediction_and_update 00218 */ 00219 virtual void prediction_and_update_pfAuxiliaryPFOptimal( 00220 const mrpt::slam::CActionCollection * action, 00221 const mrpt::slam::CSensoryFrame * observation, 00222 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00223 00224 /** Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for more information 00225 */ 00226 struct MRPTDLLIMPEXP TFastDrawAuxVars 00227 { 00228 TFastDrawAuxVars() : 00229 CDF(), 00230 CDF_indexes(), 00231 PDF(), 00232 alreadyDrawnIndexes(), 00233 alreadyDrawnNextOne(0) 00234 { } 00235 00236 vector_double CDF; 00237 vector_uint CDF_indexes; 00238 vector_double PDF; 00239 00240 vector_uint alreadyDrawnIndexes; 00241 size_t alreadyDrawnNextOne; 00242 }; 00243 00244 /** Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for more information 00245 */ 00246 mutable TFastDrawAuxVars m_fastDrawAuxiliary; 00247 00248 }; // End of class def. 00249 00250 } // end namespace 00251 } // end namespace 00252 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |