class mrpt::maps::CMultiMetricMapPDF

Overview

Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (This class is the base of RBPF-SLAM applications).

This class is used internally by the map building algorithm in “mrpt::slam::CMetricMapBuilderRBPF”

See also:

mrpt::slam::CMetricMapBuilderRBPF

#include <mrpt/maps/CMultiMetricMapPDF.h>

class CMultiMetricMapPDF:
    public mrpt::serialization::CSerializable,
    public mrpt::bayes::CParticleFilterData,
    public mrpt::bayes::CParticleFilterDataImpl,
    public mrpt::slam::PF_implementation
{
public:
    // structs

    struct TPredictionParams;

    // fields

    mrpt::maps::CMultiMetricMapPDF::TPredictionParams options;

    // construction

    CMultiMetricMapPDF();

    CMultiMetricMapPDF(
        const bayes::CParticleFilter::TParticleFilterOptions& opts,
        const mrpt::maps::TSetOfMetricMapInitializers& mapsInitializers,
        const TPredictionParams& predictionOptions
        );

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

    bool PF_SLAM_implementation_doWeHaveValidObservations(
        const 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;

    void clear(const mrpt::poses::CPose2D& initialPose);
    void clear(const mrpt::poses::CPose3D& initialPose);
    void clear(const mrpt::maps::CSimpleMap& prevMap, const mrpt::poses::CPose3D& currentPose);
    void getEstimatedPosePDFAtTime(size_t timeStep, mrpt::poses::CPose3DPDFParticles& out_estimation) const;
    void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles& out_estimation) const;
    const CMultiMetricMap* getAveragedMetricMapEstimation();
    const CMultiMetricMap* getCurrentMostLikelyMetricMap() const;
    size_t getNumberOfObservationsInSimplemap() const;
    bool insertObservation(mrpt::obs::CSensoryFrame& sf);
    void getPath(size_t i, std::deque<math::TPose3D>& out_path) const;
    double getCurrentEntropyOfPaths();
    double getCurrentJointEntropy();
    void updateSensoryFrameSequence();
    void saveCurrentPathEstimationToTextFile(const std::string& fil);
    const CParticleData* getMostLikelyParticle() const;
};

Inherited Members

public:
    // structs

    struct TFastDrawAuxVars;
    struct TMsg;

    // methods

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

Construction

CMultiMetricMapPDF()

Constructor.

Methods

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

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 bool PF_SLAM_implementation_skipRobotMovement() const

Do not move the particles until the map is populated.

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 clear(const mrpt::poses::CPose2D& initialPose)

Clear all elements of the maps, and restore all paths to a single starting pose.

void clear(const mrpt::poses::CPose3D& initialPose)

This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.

void clear(const mrpt::maps::CSimpleMap& prevMap, const mrpt::poses::CPose3D& currentPose)

Resets the map by loading an already-mapped map for past poses.

Current robot pose should be normally set to the last keyframe in the simplemap.

void getEstimatedPosePDFAtTime(size_t timeStep, mrpt::poses::CPose3DPDFParticles& out_estimation) const

Returns the estimate of the robot pose as a particles PDF for the instant of time “timeStep”, from 0 to N-1.

See also:

getEstimatedPosePDF

void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles& out_estimation) const

Returns the current estimate of the robot pose, as a particles PDF.

See also:

getEstimatedPosePDFAtTime

const CMultiMetricMap* getAveragedMetricMapEstimation()

Returns the weighted averaged map based on the current best estimation.

If you need a persistent copy of this object, please use “CSerializable::duplicate” and use the copy.

See also:

Almost 100% sure you would prefer the best current map, given by getCurrentMostLikelyMetricMap()

const CMultiMetricMap* getCurrentMostLikelyMetricMap() const

Returns a pointer to the current most likely map (associated to the most likely particle)

size_t getNumberOfObservationsInSimplemap() const

Get the number of CSensoryFrame inserted into the internal member SFs.

bool insertObservation(mrpt::obs::CSensoryFrame& sf)

Insert an observation to the map, at each particle’s pose and to each particle’s metric map.

Parameters:

sf	The SF to be inserted

Returns:

true if any may was updated, false otherwise

void getPath(size_t i, std::deque<math::TPose3D>& out_path) const

Return the path (in absolute coordinate poses) for the i’th particle.

Parameters:

On	index out of bounds

double getCurrentEntropyOfPaths()

Returns the current entropy of paths, computed as the average entropy of poses along the path, where entropy of each pose estimation is computed as the entropy of the gaussian approximation covariance.

double getCurrentJointEntropy()

Returns the joint entropy estimation over paths and maps, according to “Information Gain-based Exploration Using” by C.

Stachniss, G. Grissetti and W.Burgard.

void updateSensoryFrameSequence()

Update the poses estimation of the member “SFs” according to the current path belief.

void saveCurrentPathEstimationToTextFile(const std::string& fil)

A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively).

const CParticleData* getMostLikelyParticle() const

Returns the particle with the highest weight.