class mrpt::slam::CMonteCarloLocalization2D

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:
    //
fields

    TMonteCarloLocalizationParams options;

    // construction

    CMonteCarloLocalization2D(size_t M = 1);

    //
methods

    virtual mrpt::math::TPose3D getLastPose(const 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,
        const 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);
    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,
        const 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;

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

    template <class OPENGL_SETOFOBJECTSPTR>
    OPENGL_SETOFOBJECTSPTR getAs3DObject() const;

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

Inherited Members

public:
    // typedefs

    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

    // structs

    struct TFastDrawAuxVars;
    struct TMsg;

    //
fields

    static const particle_storage_mode PARTICLE_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(const 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,
        const 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

CMonteCarloLocalization2D(size_t M = 1)

Constructor.

Parameters:

M	The number of m_particles.

Methods

virtual mrpt::math::TPose3D getLastPose(const 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,
    const 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

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

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.

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.