class mrpt::poses::CPose3DPDFParticles

Python API: mrpt.poses.CPose3DPDFParticles

Overview

Declares a class that represents a Probability Density function (PDF) of a 3D pose.

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:

CPose3D, CPose3DPDF, CPoseGaussianPDF

#include <mrpt/poses/CPose3DPDFParticles.h>

class CPose3DPDFParticles:
    public mrpt::poses::CPose3DPDF,
    public mrpt::bayes::CParticleFilterData,
    public mrpt::bayes::CParticleFilterDataImpl
{
public:
    // typedefs

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

    // fields

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

    // construction

    CPose3DPDFParticles(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;
    virtual void copyFrom(const CPose3DPDF& o);
    void resetDeterministic(const mrpt::math::TPose3D& location, size_t particlesCount = 0);
    void resetUniform(const mrpt::math::TPose3D& corner_min, const mrpt::math::TPose3D& corner_max, const int particlesCount = -1);
    void getMean(CPose3D& mean_pose) const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    mrpt::math::TPose3D getParticlePose(int i) const;
    virtual bool saveToTextFile(const std::string& file) const;
    size_t size() const;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase);
    void drawSingleSample(CPose3D& outPart) const;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    void operator += (const CPose3D& Ap);
    void append(CPose3DPDFParticles& o);
    virtual void inverse(CPose3DPDF& o) const;
    mrpt::math::TPose3D getMostLikelyParticle() const;
    virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2);
    virtual void printTo(std::ostream& out) const;
};

// direct descendants

class CMonteCarloLocalization3D;

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<CPose3DPDF> Ptr;
    typedef std::shared_ptr<const CPose3DPDF> 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 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 CPose3DPDF& o) = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;
    virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2) = 0;
    virtual void inverse(CPose3DPDF& o) const = 0;

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

    template <class OPENGL_SETOFOBJECTSPTR>
    OPENGL_SETOFOBJECTSPTR getAs3DObject() const;

    static CPose3DPDF* createFrom2D(const CPosePDF& o);
    static void jacobiansPoseComposition(const CPose3D& x, const CPose3D& u, mrpt::math::CMatrixDouble66& df_dx, mrpt::math::CMatrixDouble66& 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 ::CPose3DPDFParticles> Ptr

A type for the associated smart pointer.

Construction

CPose3DPDFParticles(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.

virtual void copyFrom(const CPose3DPDF& o)

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

void resetDeterministic(const mrpt::math::TPose3D& 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

void resetUniform(
    const mrpt::math::TPose3D& corner_min,
    const mrpt::math::TPose3D& corner_max,
    const int particlesCount = -1
    )

Reset the PDF to an uniformly distributed one, inside of the defined “cube”.

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 resetAroundSetOfPoses

void getMean(CPose3D& mean_pose) const

Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.

See also:

getCovariance

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

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

See also:

getMean

mrpt::math::TPose3D getParticlePose(int 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 z”

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(CPose3D& outPart) const

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

virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const

Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum.

void operator += (const CPose3D& Ap)

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

void append(CPose3DPDFParticles& o)

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

virtual void inverse(CPose3DPDF& o) const

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

mrpt::math::TPose3D getMostLikelyParticle() const

Returns the particle with the highest weight.

virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2)

Bayesian fusion.

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.