class mrpt::slam::CMonteCarloLocalization2D

Python API: mrpt.slam.CMonteCarloLocalization2D

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 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(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,
        size_t particleIndexForMap,
        const mrpt::obs::CSensoryFrame& observation,
        const mrpt::poses::CPose3D& x
        ) 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);
    mrpt::viz::CSetOfObjects::Ptr getVisualization() const;
};

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

    // enums

    enum
    {
        is_3D_val = 0,
    };

    enum
    {
        is_PDF_val = 1,
    };

    // structs

    struct TFastDrawAuxVars;
    struct TMsg;

    // fields

    static constexpr size_t state_length = STATE_LEN;
    static const particle_storage_mode PARTICLE_STORAGE = STORAGE;
    CParticleList m_particles;
    static constexpr const char* className = "mrpt::poses" "::" "CPosePDFParticles";
    bool logging_enable_console_output {true};
    bool logging_enable_keep_record {false};

    // 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);
    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;
    void logStr(const VerbosityLevel level, std::string_view msg_str) const;
    void logFmt(const VerbosityLevel level, const char* fmt, ...) const;
    void void logCond(const VerbosityLevel level, bool cond, const std::string& msg_str) const;
    void setLoggerName(const std::string& name);
    std::string getLoggerName() const;
    void setVerbosityLevel(const VerbosityLevel level);
    void setVerbosityLevelForCallbacks(const VerbosityLevel level);
    void setMinLoggingLevel(const VerbosityLevel level);
    VerbosityLevel getMinLoggingLevel() const;
    VerbosityLevel getMinLoggingLevelForCallbacks() const;
    bool isLoggingLevelVisible(VerbosityLevel level) const;
    void getLogAsString(std::string& log_contents) const;
    std::string getLogAsString() const;
    void writeLogToFile(const std::optional<std::string>& fname_in = std::nullopt) const;
    void dumpLogToConsole() const;
    std::string getLoggerLastMsg() const;
    void getLoggerLastMsg(std::string& msg_str) const;
    void loggerReset();
    void logRegisterCallback(output_logger_callback_t userFunc);
    bool logDeregisterCallback(output_logger_callback_t userFunc);
    COutputLogger& operator = (const COutputLogger&);
    COutputLogger& operator = (COutputLogger&&);
    static std::array<mrpt::system::ConsoleForegroundColor, NUMBER_OF_VERBOSITY_LEVELS>& logging_levels_to_colors();
    static const std::array<const char*, NUMBER_OF_VERBOSITY_LEVELS>& logging_levels_to_names();
    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 void PF_SLAM_implementation_replaceByNewParticleSet(
        typename mrpt::bayes::CParticleFilterData<PARTICLE_TYPE, 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_doWeHaveValidObservations(
        ] const typename mrpt::bayes::CParticleFilterData<PARTICLE_TYPE, STORAGE>::CParticleList& particles,
        ] const mrpt::obs::CSensoryFrame* sf
        ) const;

    virtual bool PF_SLAM_implementation_skipRobotMovement() 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>
    bool PF_SLAM_implementation_gatherActionsCheckBothActObs(const mrpt::obs::CActionCollection* actions, const mrpt::obs::CSensoryFrame* sf);

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

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

mrpt::viz::CSetOfObjects::Ptr getVisualization() const

Returns a 3D representation of this PDF.

Needs the mrpt-opengl library, and using mrpt::viz::CSetOfObjects::Ptr as template argument.