Main MRPT website > C++ reference
MRPT logo

CParticleFilterCapable.h

Go to the documentation of this file.
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.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011