class mrpt::slam::CMonteCarloLocalization2D

Overview

Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.

This class also implements particle filtering for robot localization. See the MRPT application “app/pf-localization” for an example of usage.

See also:

CMonteCarloLocalization3D, CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable

#include <mrpt/slam/CMonteCarloLocalization2D.h>

class CMonteCarloLocalization2D:
    public mrpt::poses::CPosePDFParticles,
    public mrpt::slam::PF_implementation
{
public:
    // typedefs

    typedef T CParticleDataContent;
    typedef CProbabilityParticle<T, STORAGE> CParticleData;
    typedef std::deque<CParticleData> CParticleList;
    typedef double(*)(const bayes::CParticleFilter::TParticleFilterOptions&PF_options, const CParticleFilterCapable*obj, size_t index, const void*action, const void*observation) TParticleProbabilityEvaluator;

    // fields

    TMonteCarloLocalizationParams options;
    CParticleList m_particles;

    // construction

    CMonteCarloLocalization2D(size_t M = 1);

    // methods

    virtual mrpt::math::TPose3D getLastPose(size_t i, bool& is_valid_pose) const;

    virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(
        mrpt::math::TPose2D* particleData,
        const mrpt::math::TPose3D& newPose
        ) const;

    void PF_SLAM_implementation_replaceByNewParticleSet(
        CParticleList& old_particles,
        const std::vector<mrpt::math::TPose3D>& newParticles,
        const std::vector<double>& newParticlesWeight,
        const std::vector<size_t>& newParticlesDerivedFromIdx
        ) const;

    virtual double PF_SLAM_computeObservationLikelihoodForParticle(
        const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
        size_t particleIndexForMap,
        const mrpt::obs::CSensoryFrame& observation,
        const mrpt::poses::CPose3D& x
        ) const;

    virtual void PF_SLAM_implementation_replaceByNewParticleSet(
        typename mrpt::bayes::CParticleFilterData<mrpt::math::TPose2D, STORAGE>::CParticleList& old_particles,
        const std::vector<mrpt::math::TPose3D>& newParticles,
        const std::vector<double>& newParticlesWeight,
        const std::vector<size_t>& newParticlesDerivedFromIdx
        ) const;

    virtual bool PF_SLAM_implementation_skipRobotMovement() const;

    void resetUniformFreeSpace(
        mrpt::maps::COccupancyGridMap2D* theMap,
        const double freeCellsThreshold = 0.7,
        const int particlesCount = -1,
        const double x_min = -1e10f,
        const double x_max = 1e10f,
        const double y_min = -1e10f,
        const double y_max = 1e10f,
        const double phi_min = -M_PI,
        const double phi_max = M_PI
        );

    virtual void prediction_and_update_pfStandardProposal(const mrpt::obs::CActionCollection* action, const mrpt::obs::CSensoryFrame* observation, const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
    virtual void prediction_and_update_pfAuxiliaryPFStandard(const mrpt::obs::CActionCollection* action, const mrpt::obs::CSensoryFrame* observation, const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
    virtual void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection* action, const mrpt::obs::CSensoryFrame* observation, const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
    mrpt::opengl::CSetOfObjects::Ptr getVisualization() const;
    void clear();
    virtual void copyFrom(const CPosePDF& o);
    void resetDeterministic(const mrpt::math::TPose2D& location, size_t particlesCount = 0);

    void resetUniform(
        const double x_min,
        const double x_max,
        const double y_min,
        const double y_max,
        const double phi_min = -M_PI,
        const double phi_max = M_PI,
        const int particlesCount = -1
        );

    void resetAroundSetOfPoses(
        const std::vector<mrpt::math::TPose2D>& list_poses,
        size_t num_particles_per_pose,
        const double spread_x,
        const double spread_y,
        const double spread_phi_rad
        );

    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    mrpt::math::TPose2D getParticlePose(size_t i) const;
    virtual bool saveToTextFile(const std::string& file) const;
    size_t size() const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    void drawSingleSample(CPose2D& outPart) const;
    void operator += (const mrpt::math::TPose2D& Ap);
    void append(CPosePDFParticles& o);
    virtual void inverse(CPosePDF& o) const;
    mrpt::math::TPose2D getMostLikelyParticle() const;
    virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, const double minMahalanobisDistToDrop = 0);

    double evaluatePDF_parzen(
        const double x,
        const double y,
        const double phi,
        const double stdXY,
        const double stdPhi
        ) const;

    void saveParzenPDFToTextFile(
        const char* fileName,
        const double x_min,
        const double x_max,
        const double y_min,
        const double y_max,
        const double phi,
        const double stepSizeXY,
        const double stdXY,
        const double stdPhi
        ) const;

    virtual std::string asString() const;

    template <class OPENGL_SETOFOBJECTSPTR>
    void getAs3DObject(OPENGL_SETOFOBJECTSPTR& out_obj) const;

    template <class OPENGL_SETOFOBJECTSPTR>
    OPENGL_SETOFOBJECTSPTR getAs3DObject() const;

    void clearParticles();

    template <class STREAM>
    void writeParticlesToStream(STREAM& out) const;

    template <class STREAM>
    void readParticlesFromStream(STREAM& in);

    void getWeights(std::vector<double>& out_logWeights) const;
    const CPosePDFParticles& derived() const;
    CPosePDFParticles& derived();
    virtual double getW(size_t i) const;
    virtual void setW(size_t i, double w);
    virtual size_t particlesCount() const;
    virtual double normalizeWeights(double* out_max_log_w = nullptr);
    virtual double ESS() const;
    virtual void performSubstitution(const std::vector<size_t>& indx);

    void prepareFastDrawSample(
        const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
        TParticleProbabilityEvaluator partEvaluator = defaultEvaluator,
        const void* action = nullptr,
        const void* observation = nullptr
        ) const;

    size_t fastDrawSample(const bayes::CParticleFilter::TParticleFilterOptions& PF_options) const;
    void prediction_and_update(const mrpt::obs::CActionCollection* action, const mrpt::obs::CSensoryFrame* observation, const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
    void performResampling(const bayes::CParticleFilter::TParticleFilterOptions& PF_options, size_t out_particle_count = 0);
    bool PF_SLAM_implementation_gatherActionsCheckBothActObs(const mrpt::obs::CActionCollection* actions, const mrpt::obs::CSensoryFrame* sf);

    static void jacobiansPoseComposition(
        const CPose2D& x,
        const CPose2D& u,
        mrpt::math::CMatrixDouble33& df_dx,
        mrpt::math::CMatrixDouble33& df_du,
        const bool compute_df_dx = true,
        const bool compute_df_du = true
        );

    static void jacobiansPoseComposition(const CPosePDFGaussian& x, const CPosePDFGaussian& u, mrpt::math::CMatrixDouble33& df_dx, mrpt::math::CMatrixDouble33& df_du);

    static double defaultEvaluator(
        ] const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
        ] const CParticleFilterCapable* obj,
        size_t index,
        ] const void* action,
        ] const void* observation
        );

    static void computeResampling(
        CParticleFilter::TParticleResamplingAlgorithm method,
        const std::vector<double>& in_logWeights,
        std::vector<size_t>& out_indexes,
        size_t out_particle_count = 0
        );

    static void log2linearWeights(
        const std::vector<double>& in_logWeights,
        std::vector<double>& out_linWeights
        );
};

Inherited Members

public:
    // typedefs

    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

    // structs

    struct TFastDrawAuxVars;
    struct TMsg;

    // fields

    static const particle_storage_mode PARTICLE_STORAGE = STORAGE;

    // methods

    virtual void copyFrom(const CPosePDF& o) = 0;
    virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, const double minMahalanobisDistToDrop = 0) = 0;
    virtual void inverse(CPosePDF& o) const = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;
    virtual double getW(size_t i) const = 0;
    virtual void setW(size_t i, double w) = 0;
    virtual size_t particlesCount() const = 0;
    virtual void performSubstitution(const std::vector<size_t>& indx) = 0;
    virtual double normalizeWeights(double* out_max_log_w = nullptr) = 0;
    virtual double ESS() const = 0;
    void getMean(CPose2D& mean_pose) const;
    virtual mrpt::math::TPose3D getLastPose(size_t i, bool& is_valid_pose) const = 0;
    virtual void PF_SLAM_implementation_custom_update_particle_with_new_pose(PARTICLE_TYPE* particleData, const mrpt::math::TPose3D& newPose) const = 0;

    virtual bool PF_SLAM_implementation_doWeHaveValidObservations(
        ] const typename mrpt::bayes::CParticleFilterData<PARTICLE_TYPE, STORAGE>::CParticleList& particles,
        ] const mrpt::obs::CSensoryFrame* sf
        ) const;

    virtual double PF_SLAM_computeObservationLikelihoodForParticle(
        const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
        size_t particleIndexForMap,
        const mrpt::obs::CSensoryFrame& observation,
        const mrpt::poses::CPose3D& x
        ) const = 0;

    template <class BINTYPE>
    double PF_SLAM_particlesEvaluator_AuxPFOptimal(
        const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
        const mrpt::bayes::CParticleFilterCapable* obj,
        size_t index,
        ] const void* action,
        const void* observation
        );

Typedefs

typedef T CParticleDataContent

This is the type inside the corresponding CParticleData class.

typedef CProbabilityParticle<T, STORAGE> CParticleData

Use this to refer to each element in the m_particles array.

typedef std::deque<CParticleData> CParticleList

Use this type to refer to the list of particles m_particles.

typedef double(*)(const bayes::CParticleFilter::TParticleFilterOptions&PF_options, const CParticleFilterCapable*obj, size_t index, const void*action, const void*observation) TParticleProbabilityEvaluator

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.

Parameters:

index	This is the index of the particle its probability is being computed.
action	The value of this is the parameter passed to “prepareFastDrawSample”
observation	The value of this is the parameter passed to “prepareFastDrawSample” The action and the observation are declared as “void*” for a greater flexibility.

See also:

prepareFastDrawSample

Fields

TMonteCarloLocalizationParams options

MCL parameters.

CParticleList m_particles

The array of particles.

Construction

CMonteCarloLocalization2D(size_t M = 1)

Constructor.

Parameters:

M	The number of m_particles.

Methods

virtual mrpt::math::TPose3D getLastPose(size_t i, bool& is_valid_pose) const

Return the robot pose for the i’th particle.

is_valid is always true in this class.

virtual double PF_SLAM_computeObservationLikelihoodForParticle(
    const mrpt::bayes::CParticleFilter::TParticleFilterOptions& PF_options,
    size_t particleIndexForMap,
    const mrpt::obs::CSensoryFrame& observation,
    const mrpt::poses::CPose3D& x
    ) const

Evaluate the observation likelihood for one particle at a given location.

virtual void PF_SLAM_implementation_replaceByNewParticleSet(
    typename mrpt::bayes::CParticleFilterData<mrpt::math::TPose2D, STORAGE>::CParticleList& old_particles,
    const std::vector<mrpt::math::TPose3D>& newParticles,
    const std::vector<double>& newParticlesWeight,
    const std::vector<size_t>& newParticlesDerivedFromIdx
    ) const

This is the default algorithm to efficiently replace one old set of samples by another new set.

The method uses pointers to make fast copies the first time each particle is duplicated, then makes real copies for the next ones.

Note that more efficient specializations might exist for specific particle data structs.

virtual bool PF_SLAM_implementation_skipRobotMovement() const

Make a specialization if needed, eg.

in the first step in SLAM.

void resetUniformFreeSpace(
    mrpt::maps::COccupancyGridMap2D* theMap,
    const double freeCellsThreshold = 0.7,
    const int particlesCount = -1,
    const double x_min = -1e10f,
    const double x_max = 1e10f,
    const double y_min = -1e10f,
    const double y_max = 1e10f,
    const double phi_min = -M_PI,
    const double phi_max = M_PI
    )

Reset the PDF to an uniformly distributed one, but only in the free-space of a given 2D occupancy-grid-map.

Orientation is randomly generated in the whole 2*PI range.

Parameters:

theMap	The occupancy grid map
freeCellsThreshold	The minimum free-probability to consider a cell as empty (default is 0.7)
particlesCount	If set to -1 the number of m_particles remains unchanged.
x_min	The limits of the area to look for free cells.
x_max	The limits of the area to look for free cells.
y_min	The limits of the area to look for free cells.
y_max	The limits of the area to look for free cells.
phi_min	The limits of the area to look for free cells.
phi_max	The limits of the area to look for free cells.
std::exception	On any error (no free cell found in map, map=nullptr, etc…)

See also:

resetDeterm32inistic

virtual void prediction_and_update_pfStandardProposal(
    const mrpt::obs::CActionCollection* action,
    const mrpt::obs::CSensoryFrame* observation,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options
    )

Update the m_particles, predicting the posterior of robot pose and map after a movement command.

This method has additional configuration parameters in “options”. Performs the update stage of the RBPF, using the sensed CSensoryFrame:

Parameters:

action	This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
observation	This must be a pointer to a CSensoryFrame object, with robot sensed observations.

See also:

options

virtual void prediction_and_update_pfAuxiliaryPFStandard(
    const mrpt::obs::CActionCollection* action,
    const mrpt::obs::CSensoryFrame* observation,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options
    )

Update the m_particles, predicting the posterior of robot pose and map after a movement command.

This method has additional configuration parameters in “options”. Performs the update stage of the RBPF, using the sensed CSensoryFrame:

Parameters:

Action	This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
observation	This must be a pointer to a CSensoryFrame object, with robot sensed observations.

See also:

options

virtual void prediction_and_update_pfAuxiliaryPFOptimal(
    const mrpt::obs::CActionCollection* action,
    const mrpt::obs::CSensoryFrame* observation,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options
    )

Update the m_particles, predicting the posterior of robot pose and map after a movement command.

This method has additional configuration parameters in “options”. Performs the update stage of the RBPF, using the sensed CSensoryFrame:

Parameters:

Action	This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
observation	This must be a pointer to a CSensoryFrame object, with robot sensed observations.

See also:

options

mrpt::opengl::CSetOfObjects::Ptr getVisualization() const

Returns a 3D representation of this PDF.

Needs the mrpt-opengl library, and using mrpt::opengl::CSetOfObjects::Ptr as template argument.

void clear()

Free all the memory associated to m_particles, and set the number of parts = 0.

virtual void copyFrom(const CPosePDF& o)

Copy operator, translating if necesary (for example, between m_particles and gaussian representations)

void resetDeterministic(const mrpt::math::TPose2D& location, size_t particlesCount = 0)

Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.

Parameters:

location	The location to set all the m_particles.
particlesCount	If this is set to 0 the number of m_particles remains unchanged.

See also:

resetUniform, CMonteCarloLocalization2D::resetUniformFreeSpace, resetAroundSetOfPoses

void resetUniform(
    const double x_min,
    const double x_max,
    const double y_min,
    const double y_max,
    const double phi_min = -M_PI,
    const double phi_max = M_PI,
    const int particlesCount = -1
    )

Reset the PDF to an uniformly distributed one, inside of the defined 2D area [x_min,x_max]x[y_min,y_max] (in meters) and for orientations [phi_min, phi_max] (in radians).

Orientations can be outside of the [-pi,pi] range if so desired, but it must hold phi_max>=phi_min.

Parameters:

particlesCount

New particle count, or leave count unchanged if set to -1 (default).

See also:

resetDeterministic, CMonteCarloLocalization2D::resetUniformFreeSpace, resetAroundSetOfPoses

void resetAroundSetOfPoses(
    const std::vector<mrpt::math::TPose2D>& list_poses,
    size_t num_particles_per_pose,
    const double spread_x,
    const double spread_y,
    const double spread_phi_rad
    )

Reset the PDF to a multimodal distribution over a set of “spots” (x,y,phi) The total number of particles will be list_poses.size() * num_particles_per_pose.

Particles will be spread uniformly in a box of width spread_{x,y,phi_rad} in each of the three coordinates (meters, radians), so it can be understood as the “initial uncertainty”.

Parameters:

list_poses	The poses (x,y,phi) around which particles will be spread. Must contains at least one pose.
num_particles_per_pose	Number of particles to be spread around each of the “spots” in list_poses. Must be >=1.

See also:

resetDeterministic, CMonteCarloLocalization2D::resetUniformFreeSpace

virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const

Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.

See also:

getMean, getInformationMatrix

mrpt::math::TPose2D getParticlePose(size_t i) const

Returns the pose of the i’th particle.

virtual bool saveToTextFile(const std::string& file) const

Save PDF’s m_particles to a text file.

In each line it will go: “x y phi weight”

size_t size() const

Get the m_particles count (equivalent to “particlesCount”)

virtual void changeCoordinatesReference(const CPose3D& newReferenceBase)

this = p (+) this.

This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which “to project” the current pdf. Result PDF substituted the currently stored one in the object.

void drawSingleSample(CPose2D& outPart) const

Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)

void operator += (const mrpt::math::TPose2D& Ap)

Appends (pose-composition) a given pose “p” to each particle.

void append(CPosePDFParticles& o)

Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.

virtual void inverse(CPosePDF& o) const

Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.

mrpt::math::TPose2D getMostLikelyParticle() const

Returns the particle with the highest weight.

virtual void bayesianFusion(
    const CPosePDF& p1,
    const CPosePDF& p2,
    const double minMahalanobisDistToDrop = 0
    )

Bayesian fusion.

double evaluatePDF_parzen(
    const double x,
    const double y,
    const double phi,
    const double stdXY,
    const double stdPhi
    ) const

Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window.

See also:

saveParzenPDFToTextFile

void saveParzenPDFToTextFile(
    const char* fileName,
    const double x_min,
    const double x_max,
    const double y_min,
    const double y_max,
    const double phi,
    const double stepSizeXY,
    const double stdXY,
    const double stdPhi
    ) const

Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window.

See also:

evaluatePDF_parzen

virtual std::string asString() const

Returns a human-friendly textual description of the object.

For classes with a large/complex internal state, only a summary should be returned instead of the exhaustive enumeration of all data members.

template <class OPENGL_SETOFOBJECTSPTR>
void getAs3DObject(OPENGL_SETOFOBJECTSPTR& out_obj) const

Returns a 3D representation of this PDF (it doesn’t clear the current contents of out_obj, but append new OpenGL objects to that list)

Needs the mrpt-opengl library, and using mrpt::opengl::CSetOfObjects::Ptr as template argument.

By default, ellipsoids for the confidence intervals of “q=3” are drawn; for more mathematical details, see CGeneralizedEllipsoidTemplate::setQuantiles()

template <class OPENGL_SETOFOBJECTSPTR>
OPENGL_SETOFOBJECTSPTR getAs3DObject() const

Returns a 3D representation of this PDF.

Needs the mrpt-opengl library, and using mrpt::opengl::CSetOfObjects::Ptr as template argument.

void clearParticles()

Free the memory of all the particles and reset the array “m_particles” to length zero

template <class STREAM>
void writeParticlesToStream(STREAM& out) const

Dumps the sequence of particles and their weights to a stream (requires T implementing CSerializable).

See also:

readParticlesFromStream

template <class STREAM>
void readParticlesFromStream(STREAM& in)

Reads the sequence of particles and their weights from a stream (requires T implementing CSerializable).

See also:

writeParticlesToStream

void getWeights(std::vector<double>& out_logWeights) const

Returns a vector with the sequence of the logaritmic weights of all the samples.

const CPosePDFParticles& derived() const

CRTP helper method.

CPosePDFParticles& derived()

CRTP helper method.

virtual double getW(size_t i) const

Access to i’th particle (logarithm) weight, where first one is index 0.

virtual void setW(size_t i, double w)

Modifies i’th particle (logarithm) weight, where first one is index 0.

virtual size_t particlesCount() const

Get the m_particles count.

virtual double normalizeWeights(double* out_max_log_w = nullptr)

Normalize the (logarithmic) weights, such as the maximum weight is zero.

Parameters:

out_max_log_w

If provided, will return with the maximum log_w before normalizing, such as new_weights = old_weights - max_log_w.

Returns:

The max/min ratio of weights (“dynamic range”)

virtual double ESS() const

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 void performSubstitution(const std::vector<size_t>& indx)

Replaces the old particles by copies determined by the indexes in “indx”, performing an efficient copy of the necesary particles only and allowing the number of particles to change.

void prepareFastDrawSample(
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options,
    TParticleProbabilityEvaluator partEvaluator = defaultEvaluator,
    const void* action = nullptr,
    const void* observation = nullptr
    ) const

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):

DYNAMIC SAMPLE SIZE=NO : 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.
DYNAMIC SAMPLE SIZE=YES : 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 Particle Filter tutorial. Custom supplied “partEvaluator” functions must take into account the previous particle weight, i.e. multiplying the current observation likelihood by the weights.

See also:

fastDrawSample

size_t fastDrawSample(const bayes::CParticleFilter::TParticleFilterOptions& PF_options) 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 Particle Filter tutorial.

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.

See also:

prepareFastDrawSample

void prediction_and_update(
    const mrpt::obs::CActionCollection* action,
    const mrpt::obs::CSensoryFrame* observation,
    const bayes::CParticleFilter::TParticleFilterOptions& PF_options
    )

Performs the prediction stage of the Particle Filter.

This method simply selects the appropiate protected method according to the particle filter algorithm to run.

See also:

prediction_and_update_pfStandardProposal, prediction_and_update_pfAuxiliaryPFStandard, prediction_and_update_pfOptimalProposal, prediction_and_update_pfAuxiliaryPFOptimal

void performResampling(const bayes::CParticleFilter::TParticleFilterOptions& PF_options, size_t out_particle_count = 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”

Parameters:

out_particle_count

The desired number of output particles after resampling; 0 means don’t modify the current number.

See also:

resample

bool PF_SLAM_implementation_gatherActionsCheckBothActObs(
    const mrpt::obs::CActionCollection* actions,
    const mrpt::obs::CSensoryFrame* sf
    )

Auxiliary method called by PF implementations: return true if we have both action & observation, otherwise, return false AND accumulate the odometry so when we have an observation we didn’t lose a thing.

On return=true, the “m_movementDrawer” member is loaded and ready to draw samples of the increment of pose since last step. This method is smart enough to accumulate CActionRobotMovement2D or CActionRobotMovement3D, whatever comes in.

static void jacobiansPoseComposition(
    const CPose2D& x,
    const CPose2D& u,
    mrpt::math::CMatrixDouble33& df_dx,
    mrpt::math::CMatrixDouble33& df_du,
    const bool compute_df_dx = true,
    const bool compute_df_du = true
    )

This static method computes the pose composition Jacobians, with these formulas:

df_dx =
[ 1, 0, -sin(phi_x)*x_u-cos(phi_x)*y_u ]
[ 0, 1,  cos(phi_x)*x_u-sin(phi_x)*y_u ]
[ 0, 0,                              1 ]

df_du =
[ cos(phi_x) , -sin(phi_x) ,  0  ]
[ sin(phi_x) ,  cos(phi_x) ,  0  ]
[         0  ,          0  ,  1  ]

static void jacobiansPoseComposition(const CPosePDFGaussian& x, const CPosePDFGaussian& u, mrpt::math::CMatrixDouble33& df_dx, mrpt::math::CMatrixDouble33& df_du)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

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.

The action and the observation are declared as “void*” for a greater flexibility.

See also:

prepareFastDrawSample

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 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

Parameters:

out_particle_count

The desired number of output particles after resampling; 0 means don’t modify the current number.

See also:

performResampling

static void log2linearWeights(
    const std::vector<double>& in_logWeights,
    std::vector<double>& out_linWeights
    )

A static method to compute the linear, normalized (the sum the unity) weights from log-weights.

See also:

performResampling