class mrpt::poses::CPointPDFParticles

Overview

A probability distribution of a 2D/3D point, represented as a set of random samples (particles).

See also:

CPointPDF

#include <mrpt/poses/CPointPDFParticles.h>

class CPointPDFParticles:
    public mrpt::poses::CPointPDF,
    public mrpt::bayes::CParticleFilterData,
    public mrpt::bayes::CParticleFilterDataImpl
{
public:
    // typedefs

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

    // fields

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

    // construction

    CPointPDFParticles(size_t numParticles = 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();
    void setSize(size_t numberParticles, const mrpt::math::TPoint3Df& defaultValue = mrpt::math::TPoint3Df{0, 0, 0});
    size_t size() const;
    void getMean(CPoint3D& mean_point) const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    virtual void copyFrom(const CPointPDF& o);
    virtual bool saveToTextFile(const std::string& file) const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    double computeKurtosis();
    void drawSingleSample(CPoint3D& outSample) const;
    virtual void bayesianFusion(const CPointPDF& p1, const CPointPDF& p2, const double minMahalanobisDistToDrop = 0);
};

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<CPointPDF> Ptr;
    typedef std::shared_ptr<const CPointPDF> 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 = 1,
    };

    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 const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    virtual void copyFrom(const CPointPDF& o) = 0;
    virtual void bayesianFusion(const CPointPDF& p1, const CPointPDF& p2, const double minMahalanobisDistToDrop = 0) = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;

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

    template <class OPENGL_SETOFOBJECTSPTR, class OPENGL_SETOFOBJECTS>
    OPENGL_SETOFOBJECTSPTR getAs3DObject() const;

    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 ::CPointPDFParticles> Ptr

A type for the associated smart pointer.

Construction

CPointPDFParticles(size_t numParticles = 1)

Default constructor.

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

Clear all the particles (free memory)

void setSize(size_t numberParticles, const mrpt::math::TPoint3Df& defaultValue = mrpt::math::TPoint3Df{0, 0, 0})

Erase all the previous particles and change the number of particles, with a given initial value

size_t size() const

Returns the number of particles.

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

virtual void copyFrom(const CPointPDF& o)

Copy operator, translating if necessary (for example, between particles and gaussian representations)

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

Save PDF’s particles to a text file, where each line is: X Y Z LOG_W.

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. Both the mean value and the covariance matrix are updated correctly.

double computeKurtosis()

Compute the kurtosis of the distribution.

void drawSingleSample(CPoint3D& outSample) const

Draw a sample from the pdf.

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

Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)

Parameters:

p1	The first distribution to fuse
p2	The second distribution to fuse
minMahalanobisDistToDrop	If set to different of 0, the result of very separate Gaussian modes (that will result in negligible components) in SOGs will be dropped to reduce the number of modes in the output.