CParticleFilterCapable.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#ifndef CPARTICLEFILTERCAPABLE_H
#define CPARTICLEFILTERCAPABLE_H

#include <mrpt/utils/utils_defs.h>
#include <mrpt/bayes/CParticleFilter.h>

#include <functional>

namespace mrpt
{
namespace bayes
{
#define INVALID_LIKELIHOOD_VALUE \
        (-1e300)  // An invalid log-likelihood value, used to signal non-initialized
// likelihood variables.

/** This virtual class defines the interface that any particles based PDF class
 * must implement in order to be executed by a mrpt::bayes::CParticleFilter.
 *
 * See the <a href="http://www.mrpt.org/Particle_Filter_Tutorial" >Particle
 * Filter tutorial</a> explaining how to use the particle filter-related
 * classes.
 * \sa CParticleFilter, CParticleFilterData
 * \ingroup mrpt_base_grp
 */
class CParticleFilterCapable
{
        friend class CParticleFilter;

   private:
        static const unsigned PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;

   public:
        CParticleFilterCapable() : m_fastDrawAuxiliary() {}
        /** Virtual destructor
         */
        virtual ~CParticleFilterCapable() {}
        /** A callback function type for evaluating the probability of m_particles
         * of being selected, used in "fastDrawSample".
         *  The default evaluator function "defaultEvaluator" simply returns the
         * particle weight.
         * \param index This is the index of the particle its probability is being
         * computed.
         * \param action The value of this is the parameter passed to
         * "prepareFastDrawSample"
         * \param observation The value of this is the parameter passed to
         * "prepareFastDrawSample"
         *  The action and the observation are declared as "void*" for a greater
         * flexibility.
         * \sa prepareFastDrawSample
         */
        using TParticleProbabilityEvaluator = std::function<double(size_t index)>;

        /** The default evaluator function, which simply returns the particle
         * weight.
         *  The action and the observation are declared as "void*" for a greater
         * flexibility.
         * \sa prepareFastDrawSample
         */
        void prepareFastDrawSample(
                const bayes::CParticleFilter::TParticleFilterOptions& PF_options) const
        {
                prepareFastDrawSample(PF_options, [this](size_t i) { return getW(i); });
        }

        /** Prepares data structures for calling fastDrawSample method next.
         *  This method must be called once before using "fastDrawSample" (calling
         *this more than once has no effect, but it takes time for nothing!)
         *  The behavior depends on the configuration of the PF (see
         *CParticleFilter::TParticleFilterOptions):
         *              - <b>DYNAMIC SAMPLE SIZE=NO</b>: In this case this method fills out
         *an
         *internal array (m_fastDrawAuxiliary.alreadyDrawnIndexes) with
         *                      the random indexes generated according to the selected resample
         *scheme
         *in TParticleFilterOptions. Those indexes are
         *                      read sequentially by subsequent calls to fastDrawSample.
         *              - <b>DYNAMIC SAMPLE SIZE=YES</b>: Then:
         *                      - If TParticleFilterOptions.resamplingMethod = prMultinomial,
         *the
         *internal buffers will be filled out (m_fastDrawAuxiliary.CDF, CDF_indexes
         *& PDF) and
         *                              then fastDrawSample can be called an arbitrary number of
         *times
         *to
         *generate random indexes.
         *                      - 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.
         *
         * The function pointed by "partEvaluator" should take into account the
         *particle filter algorithm selected in "m_PFAlgorithm".
         * If called without arguments (defaultEvaluator), the default behavior is
         *to draw samples with a probability proportional to their current weights.
         *  The action and the observation are declared as "void*" for a greater
         *flexibility.
         *  For a more detailed information see the <a
         *href="http://www.mrpt.org/Particle_Filters" >Particle Filter
         *tutorial</a>.
         *  Custom supplied "partEvaluator" functions must take into account the
         *previous particle weight, i.e. multiplying the current observation
         *likelihood by the weights.
         * \sa fastDrawSample
         */
        void prepareFastDrawSample(
                const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
                TParticleProbabilityEvaluator partEvaluator) const;

        /** 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).
         *   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.
         *   See prepareFastDrawSample for more information, or the <a
         *href="http://www.mrpt.org/Particle_Filters" >Particle Filter
         *tutorial</a>.
         *
         * NOTES:
         *              - You MUST call "prepareFastDrawSample" ONCE before calling this
         *method. That method must be called after modifying the particle filter
         *(executing one step, resampling, etc...)
         *              - This method returns ONE index for the selected ("drawn") particle,
         *in
         *the range [0,M-1]
         *              - You do not need to call "normalizeWeights" before calling this.
         * \sa prepareFastDrawSample
         */
        size_t fastDrawSample(
                const bayes::CParticleFilter::TParticleFilterOptions& PF_options) const;

        /** Access to i'th particle (logarithm) weight, where first one is index 0.
         */
        virtual double getW(size_t i) const = 0;

        /** Modifies i'th particle (logarithm) weight, where first one is index 0.
         */
        virtual void setW(size_t i, double w) = 0;

        /** Get the m_particles count.
         */
        virtual size_t particlesCount() const = 0;

        /**  Performs the substitution for internal use of resample in particle
         * filter algorithm, don't call it directly.
         *  \param indx The indices of current m_particles to be saved as the new
         * m_particles set.
         */
        virtual void performSubstitution(const std::vector<size_t>& indx) = 0;

        /** Normalize the (logarithmic) weights, such as the maximum weight is zero.
         * \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.
         * \return The max/min ratio of weights ("dynamic range")
         */
        virtual double normalizeWeights(double* out_max_log_w = nullptr) = 0;

        /** Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
         *  Note that you do NOT need to normalize the weights before calling this.
         */
        virtual double ESS() const = 0;

        /** Performs a resample of the m_particles, using the method selected in the
         * constructor.
         * After computing the surviving samples, this method internally calls
         * "performSubstitution" to actually perform the particle replacement.
         * This method is called automatically by CParticleFilter::execute,
         * andshould not be invoked manually normally.
         * To just obtaining the sequence of resampled indexes from a sequence of
         * weights, use "resample"
         * \param[in] out_particle_count The desired number of output particles
         * after resampling; 0 means don't modify the current number.
         * \sa resample
         */
        void performResampling(
                const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
                size_t out_particle_count = 0);

        /** 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.
         * It returns the sequence of indexes from the resampling. The number of
         * output samples is the same than the input population.
         *  This generic method just computes these indexes, to actually perform a
         * resampling in a particle filter object, call performResampling
         * \param[in] out_particle_count The desired number of output particles
         * after resampling; 0 means don't modify the current number.
         * \sa performResampling
         */
        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 compute the linear, normalized (the sum the unity)
         * weights from log-weights.
         * \sa performResampling
         */
        static void log2linearWeights(
                const std::vector<double>& in_logWeights,
                std::vector<double>& out_linWeights);

   protected:
        /** Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for
         * more information
         */
        struct TFastDrawAuxVars
        {
                TFastDrawAuxVars()
                        : CDF(),
                          CDF_indexes(),
                          PDF(),
                          alreadyDrawnIndexes(),
                          alreadyDrawnNextOne(0)
                {
                }

                std::vector<double> CDF;
                vector_uint CDF_indexes;
                std::vector<double> PDF;

                vector_uint alreadyDrawnIndexes;
                size_t alreadyDrawnNextOne;
        };

        /** Auxiliary vectors, see CParticleFilterCapable::prepareFastDrawSample for
         * more information
         */
        mutable TFastDrawAuxVars m_fastDrawAuxiliary;

};  // End of class def.

}  // namespace bayes
}  // namespace mrpt
#endif