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