template class mrpt::slam::PF_implementation

Overview

A set of common data shared by PF implementations for both SLAM and localization.

#include <mrpt/slam/PF_implementations_data.h>

template <
    class PARTICLE_TYPE,
    class MYSELF,
    mrpt::bayes::particle_storage_mode STORAGE
    >
class PF_implementation: public mrpt::system::COutputLogger
{
public:
    // construction

    PF_implementation();

    // methods

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

// direct descendants

class CMonteCarloLocalization2D;
class CMonteCarloLocalization3D;
class CMultiMetricMapPDF;

Inherited Members

public:
    // structs

    struct TMsg;

    // fields

    bool logging_enable_console_output {true};
    bool logging_enable_keep_record {false};

    // methods

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

Methods

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

Return the last robot pose in the i’th particle; is_valid_pose will be false if there is no such last pose.

Parameters:

std::exception

on out-of-range particle index

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

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.

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

Evaluate the observation likelihood for one particle at a given location.

template <class BINTYPE>
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.