class mrpt::slam::CMonteCarloLocalization3D

Overview

Declares a class that represents a Probability Density Function (PDF) over a 3D pose (x,y,phi,yaw,pitch,roll), 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:

CMonteCarloLocalization2D, CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable

#include <mrpt/slam/CMonteCarloLocalization3D.h>

class CMonteCarloLocalization3D:
    public mrpt::poses::CPose3DPDFParticles,
    public mrpt::slam::PF_implementation
{
public:
    // fields

    TMonteCarloLocalizationParams options;

    // construction

    CMonteCarloLocalization3D(size_t M = 1);

    // methods

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

    void PF_SLAM_implementation_custom_update_particle_with_new_pose(
        CParticleDataContent* 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 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;
    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 std::string asString() const;

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

Inherited Members

public:
    // typedefs

    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

    // structs

    struct TFastDrawAuxVars;
    struct TMsg;

    // methods

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

Fields

TMonteCarloLocalizationParams options

MCL parameters.

Construction

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

virtual void copyFrom(const CPose3DPDF& o)

Copy operator, translating if necesary (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 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()

See also:

mrpt::opengl::CSetOfObjects::posePDF2opengl() for details on pose-to-opengl conversion

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.

static CPose3DPDF* createFrom2D(const CPosePDF& o)

This is a static transformation method from 2D poses to 3D PDFs, preserving the representation type (particles->particles, Gaussians->Gaussians,etc)

It returns a new object of any of the derived classes of CPose3DPDF. This object must be deleted by the user when not required anymore.

See also:

copyFrom

static void jacobiansPoseComposition(const CPose3D& x, const CPose3D& u, mrpt::math::CMatrixDouble66& df_dx, mrpt::math::CMatrixDouble66& df_du)

This static method computes the pose composition Jacobians.

See this technical report: http:///www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty

Direct equations (for the covariances) in yaw-pitch-roll are too complex. Make a way around them and consider instead this path:

X(6D)       U(6D)
  |           |
  v           v
X(7D)       U(7D)
  |           |
  +--- (+) ---+
        |
        v
      RES(7D)
        |
        v
      RES(6D)