class mrpt::poses::CPosePDFParticles

Python API: mrpt.poses.CPosePDFParticles

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 is also the base for the implementation of Monte-Carlo Localization (MCL), in mrpt::slam::CMonteCarloLocalization2D.

See the application “app/pf-localization” for an example of usage.

See also:

CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable

#include <mrpt/poses/CPosePDFParticles.h>

class CPosePDFParticles:
    public mrpt::poses::CPosePDF,
    public mrpt::bayes::CParticleFilterData,
    public mrpt::bayes::CParticleFilterDataImpl
{
public:
    // typedefs

    typedef std::shared_ptr<mrpt::poses ::CPosePDFParticles> Ptr;
    typedef std::shared_ptr<const mrpt::poses ::CPosePDFParticles> ConstPtr;
    typedef std::unique_ptr<mrpt::poses ::CPosePDFParticles> UniquePtr;
    typedef std::unique_ptr<const mrpt::poses ::CPosePDFParticles> ConstUniquePtr;

    // fields

    static constexpr const char* className = "mrpt::poses" "::" "CPosePDFParticles";

    // construction

    CPosePDFParticles(size_t M = 1);

    // methods

    static constexpr auto getClassName();
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    static std::shared_ptr<CObject> CreateObject();

    template <typename... Args>
    static Ptr Create(Args&&... args);

    template <typename Alloc, typename... Args>
    static Ptr CreateAlloc(
        const Alloc& alloc,
        Args&&... args
        );

    template <typename... Args>
    static UniquePtr CreateUnique(Args&&... args);

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual mrpt::rtti::CObject* clone() 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
        );

    void getMean(CPose2D& mean_pose) const;
    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 void printTo(std::ostream& out) const;
};

// direct descendants

class CMonteCarloLocalization2D;

Inherited Members

public:
    // typedefs

    typedef std::shared_ptr<CObject> Ptr;
    typedef std::shared_ptr<const CObject> ConstPtr;
    typedef std::unique_ptr<CObject> UniquePtr;
    typedef std::unique_ptr<const CObject> ConstUniquePtr;
    typedef std::shared_ptr<CSerializable> Ptr;
    typedef std::shared_ptr<const CSerializable> ConstPtr;
    typedef TDATA type_value;
    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;
    typedef mrpt::math::CMatrixFixed<double, STATE_LEN, STATE_LEN> cov_mat_t;
    typedef cov_mat_t inf_mat_t;
    typedef std::shared_ptr<CPosePDF> Ptr;
    typedef std::shared_ptr<const CPosePDF> ConstPtr;
    typedef T CParticleDataContent;
    typedef CProbabilityParticle<T, STORAGE> CParticleData;
    typedef std::deque<CParticleData> CParticleList;
    typedef std::function<double(const bayes::CParticleFilter::TParticleFilterOptions&PF_options, const CParticleFilterCapable*obj, size_t index, const void*action, const void*observation)> TParticleProbabilityEvaluator;

    // enums

    enum
    {
        is_3D_val = 0,
    };

    enum
    {
        is_PDF_val = 1,
    };

    // structs

    struct TFastDrawAuxVars;

    // fields

    static constexpr size_t state_length = STATE_LEN;
    static const particle_storage_mode PARTICLE_STORAGE = STORAGE;
    CParticleList m_particles;

    // methods

    mrpt::rtti::CObject::Ptr duplicateGetSmartPtr() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    virtual CObject* clone() const = 0;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    CProbabilityDensityFunction& operator = (const CProbabilityDensityFunction&);
    CProbabilityDensityFunction& operator = (CProbabilityDensityFunction&&);
    virtual void getMean(type_value& mean_point) const = 0;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const = 0;
    virtual void getCovarianceAndMean(cov_mat_t& c, TDATA& mean) const;
    void getCovarianceDynAndMean(mrpt::math::CMatrixDouble& cov, type_value& mean_point) const;
    type_value getMeanVal() const;
    void getCovariance(mrpt::math::CMatrixDouble& cov) const;
    void getCovariance(cov_mat_t& cov) const;
    cov_mat_t getCovariance() const;
    virtual bool isInfType() const;
    virtual void getInformationMatrix(inf_mat_t& inf) const;
    virtual bool saveToTextFile(const std::string& file) const = 0;
    virtual void drawSingleSample(TDATA& outPart) const = 0;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    double getCovarianceEntropy() const;
    virtual std::string asString() const = 0;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual void printTo(std::ostream& out) const = 0;
    virtual std::string asString() const;
    virtual void copyFrom(const CPosePDF& o) = 0;
    virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, double minMahalanobisDistToDrop = 0) = 0;
    virtual void inverse(CPosePDF& o) const = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;

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

    template <class OPENGL_SETOFOBJECTSPTR>
    OPENGL_SETOFOBJECTSPTR getAs3DObject() const;

    static void jacobiansPoseComposition(
        const CPose2D& x,
        const CPose2D& u,
        mrpt::math::CMatrixDouble33& df_dx,
        mrpt::math::CMatrixDouble33& df_du,
        bool compute_df_dx = true,
        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 constexpr bool is_3D();
    static constexpr bool is_PDF();
    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;
    std::vector<double> getWeights() const;
    const CParticleData* getMostLikelyParticle() const;

    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;
    virtual double getW(size_t i) const = 0;
    virtual void setW(size_t i, double w) = 0;
    virtual size_t particlesCount() const = 0;
    void prediction_and_update(const mrpt::obs::CActionCollection* action, const mrpt::obs::CSensoryFrame* observation, const bayes::CParticleFilter::TParticleFilterOptions& PF_options);
    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 performResampling(const bayes::CParticleFilter::TParticleFilterOptions& PF_options, size_t out_particle_count = 0);

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

    static std::vector<double> logWeightsToLinear(const std::vector<double>& in_logWeights);
    const Derived& derived() const;
    Derived& 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);

Typedefs

typedef std::shared_ptr<mrpt::poses ::CPosePDFParticles> Ptr

A type for the associated smart pointer.

Construction

CPosePDFParticles(size_t M = 1)

Constructor.

Parameters:

M	The number of m_particles.

Methods

virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const

Returns information about the class of an object in runtime.

virtual mrpt::rtti::CObject* clone() const

Returns a deep copy (clone) of the object, indepently of its class.

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 necessary (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 void printTo(std::ostream& out) const

Write a human-readable description of this PDF to the given stream.

Derived classes must override this method.