class mrpt::maps::CRandomFieldGridMap2D

Overview

CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property which is estimated by this map, either as a simple value or as a probility distribution (for each cell).

There are a number of methods available to build the MRF grid-map, depending on the value of TMapRepresentation maptype passed in the constructor.

The following papers describe the mapping alternatives implemented here:

• mrKernelDM : A Gaussian kernel-based method. See:

  • “Building gas concentration gridmaps with a mobile robot”, Lilienthal, A. and Duckett, T., Robotics and Autonomous Systems, v.48, 2004.

• mrKernelDMV : A kernel-based method. See:

  • “A Statistical Approach to Gas Distribution Modelling with Mobile Robots–The Kernel DM+ V Algorithm”, Lilienthal, A.J. and Reggente, M. and Trincavelli, M. and Blanco, J.L. and Gonzalez, J., IROS 2009.

• mrKalmanFilter : A “brute-force” approach to estimate the entire map with a dense (linear) Kalman filter. Will be very slow for mid or large maps. It’s provided just for comparison purposes, not useful in practice.

• mrKalmanApproximate : A compressed/sparse Kalman filter approach. See:

  • “A Kalman Filter Based Approach to Probabilistic Gas Distribution Mapping”, JL Blanco, JG Monroy, J Gonzalez-Jimenez, A Lilienthal, 28th Symposium On Applied Computing (SAC), 2013.

• mrGMRF_SD : A Gaussian Markov Random Field (GMRF) estimator, with these constraints:

  • mrGMRF_SD : Each cell only connected to its 4 immediate neighbors (Up, down, left, right).

  • (Removed in MRPT 1.5.0: mrGMRF_G : Each cell connected to a square area of neighbors cells)

  • See papers:

    • “Time-variant gas distribution mapping with obstacle information”, Monroy, J. G., Blanco, J. L., & Gonzalez-Jimenez, J. Autonomous Robots, 40(1), 1-16, 2016.

Note that this class is virtual, since derived classes still have to implement:

• mrpt::maps::CMetricMap::internal_computeObservationLikelihood()

• mrpt::maps::CMetricMap::internal_insertObservation()

• Serialization methods: writeToStream() and readFromStream()

[GMRF only] A custom connectivity pattern between cells can be defined by calling setCellsConnectivity().

See also:

mrpt::maps::CGasConcentrationGridMap2D, mrpt::maps::CWirelessPowerGridMap2D, mrpt::maps::CMetricMap, mrpt::containers::CDynamicGrid, The application icp-slam, mrpt::maps::CMultiMetricMap

#include <mrpt/maps/CRandomFieldGridMap2D.h>

class CRandomFieldGridMap2D:
    public mrpt::maps::CMetricMap,
    public mrpt::containers::CDynamicGrid,
    public mrpt::system::COutputLogger
{
public:
    // typedefs

    typedef std::shared_ptr<CRandomFieldGridMap2D> Ptr;
    typedef std::shared_ptr<const CRandomFieldGridMap2D> ConstPtr;

    // enums

    enum TGridInterpolationMethod;
    enum TMapRepresentation;

    // structs

    struct ConnectivityDescriptor;
    struct TInsertionOptionsCommon;
    struct TObservationGMRF;
    struct TPriorFactorGMRF;

    // construction

    CRandomFieldGridMap2D(
        TMapRepresentation mapType = mrKernelDM,
        double x_min = -2,
        double x_max = 2,
        double y_min = -2,
        double y_max = 2,
        double resolution = 0.1
        );

    // methods

    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    void clear();
    float cell2float(const TRandomFieldCell& c) const;
    virtual std::string asString() const;
    virtual bool isEmpty() const;
    virtual void saveAsBitmapFile(const std::string& filName) const;
    virtual void getAsBitmapFile(mrpt::img::CImage& out_img) const;
    virtual void getAsMatrix(mrpt::math::CMatrixDouble& out_mat) const;

    void resize(
        double new_x_min,
        double new_x_max,
        double new_y_min,
        double new_y_max,
        const TRandomFieldCell& defaultValueNewCells,
        double additionalMarginMeters = 1.0f
        );

    virtual void setSize(
        const double x_min,
        const double x_max,
        const double y_min,
        const double y_max,
        const double resolution,
        const TRandomFieldCell* fill_value = nullptr
        );

    void setCellsConnectivity(const ConnectivityDescriptor::Ptr& new_connectivity_descriptor);
    virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap* otherMap, const mrpt::poses::CPose3D& otherMapPose, const TMatchingRatioParams& params) const;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    virtual void saveAsMatlab3DGraph(const std::string& filName) const;
    virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& outObj) const;
    virtual void getAs3DObject(mrpt::viz::CSetOfObjects& meanObj, mrpt::viz::CSetOfObjects& varObj) const;
    TMapRepresentation getMapType();

    void insertIndividualReading(
        const double sensorReading,
        const mrpt::math::TPoint2D& point,
        const bool update_map = true,
        const bool time_invariant = true,
        const double reading_stddev = .0
        );

    virtual void predictMeasurement(
        const double x,
        const double y,
        double& out_predict_response,
        double& out_predict_response_variance,
        bool do_sensor_normalization,
        const TGridInterpolationMethod interp_method = gimNearest
        );

    void getMeanAndCov(mrpt::math::CVectorDouble& out_means, mrpt::math::CMatrixDouble& out_cov) const;
    void getMeanAndSTD(mrpt::math::CVectorDouble& out_means, mrpt::math::CVectorDouble& out_STD) const;
    void setMeanAndSTD(mrpt::math::CVectorDouble& out_means, mrpt::math::CVectorDouble& out_STD);
    void updateMapEstimation();
    void enableVerbose(] bool enable_verbose);
    bool isEnabledVerbose() const;
    void enableProfiler(bool enable = true);
    bool isProfilerEnabled() const;
};

// direct descendants

class CGasConcentrationGridMap2D;
class CHeightGridMap2D_MRF;
class CWirelessPowerGridMap2D;

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 std::shared_ptr<CMetricMap> Ptr;
    typedef std::shared_ptr<const CMetricMap> ConstPtr;
    typedef std::vector<T> grid_data_t;
    typedef typename grid_data_t::iterator iterator;
    typedef typename grid_data_t::const_iterator const_iterator;

    // structs

    struct TMsg;

    // fields

    TMapGenericParams genericMapParams;
    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();
    virtual std::string asString() const = 0;
    Visualizable& operator = (const Visualizable&);
    Visualizable& operator = (Visualizable&&);
    virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& o) const = 0;
    std::shared_ptr<mrpt::viz::CSetOfObjects> getVisualization() const;
    virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const;
    static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic();
    void clear();
    virtual bool isEmpty() const = 0;
    virtual auto boundingBox() const;
    void loadFromSimpleMap(const mrpt::maps::CSimpleMap& Map);
    bool insertObservation(const mrpt::obs::CObservation& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt);
    bool insertObservationPtr(const mrpt::obs::CObservation::Ptr& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt);
    double computeObservationLikelihood(const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const;
    virtual bool canComputeObservationLikelihood(const mrpt::obs::CObservation& obs) const;
    double computeObservationsLikelihood(const mrpt::obs::CSensoryFrame& sf, const mrpt::poses::CPose3D& takenFrom);
    bool canComputeObservationsLikelihood(const mrpt::obs::CSensoryFrame& sf) const;

    virtual void determineMatching2D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose2D& otherMapPose,
        mrpt::tfest::TMatchingPairList& correspondences,
        const TMatchingParams& params,
        TMatchingExtraResults& extraResults
        ) const;

    virtual void determineMatching3D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose3D& otherMapPose,
        mrpt::tfest::TMatchingPairList& correspondences,
        const TMatchingParams& params,
        TMatchingExtraResults& extraResults
        ) const;

    virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap* otherMap, const mrpt::poses::CPose3D& otherMapPose, const TMatchingRatioParams& params) const;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const = 0;
    virtual void auxParticleFilterCleanUp();
    virtual float squareDistanceToClosestCorrespondence(float x0, float y0) const;
    const grid_data_t& data() const;
    iterator begin();
    iterator end();
    const_iterator begin() const;
    const_iterator end() const;

    void setSize(
        const double x_min,
        const double x_max,
        const double y_min,
        const double y_max,
        const double resolution,
        const T* fill_value = nullptr
        );

    void clear();
    void fill(const T& value);

    virtual void resize(
        double new_x_min,
        double new_x_max,
        double new_y_min,
        double new_y_max,
        const T& defaultValueNewCells,
        double additionalMarginMeters = 2.0
        );

    T* cellByPos(double x, double y);
    const T* cellByPos(double x, double y) const;
    T* cellByIndex(unsigned int cx, unsigned int cy);
    const T* cellByIndex(unsigned int cx, unsigned int cy) const;
    size_t getSizeX() const;
    size_t getSizeY() const;
    double getXMin() const;
    double getXMax() const;
    double getYMin() const;
    double getYMax() const;
    double getResolution() const;
    int x2idx(double x) const;
    int y2idx(double y) const;
    int xy2idx(double x, double y) const;
    void idx2cxcy(int idx, int& cx, int& cy) const;
    double idx2x(int cx) const;
    double idx2y(int cy) const;

    template <class MAT>
    void getAsMatrix(MAT& m) const;

    virtual float cell2float(const T&) const;
    bool saveToTextFile(const std::string& fileName) 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();

Construction

CRandomFieldGridMap2D(
    TMapRepresentation mapType = mrKernelDM,
    double x_min = -2,
    double x_max = 2,
    double y_min = -2,
    double y_max = 2,
    double resolution = 0.1
    )

Constructor.

Methods

virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const

Returns information about the class of an object in runtime.

void clear()

Calls the base CMetricMap::clear Declared here to avoid ambiguity between the two clear() in both base classes.

virtual std::string asString() const

Returns a short description of the map.

virtual bool isEmpty() const

Returns true if the map is empty/no observation has been inserted (in this class it always return false, unless redefined otherwise in base classes)

virtual void saveAsBitmapFile(const std::string& filName) const

Save the current map as a graphical file (BMP,PNG,…).

The file format will be derived from the file extension (see CImage::saveToFile ) It depends on the map representation model: mrAchim: Each pixel is the ratio \(\sum{\frac{wR}{w}}\) mrKalmanFilter: Each pixel is the mean value of the Gaussian that represents each cell.

See also:

getAsBitmapFile()

virtual void getAsBitmapFile(mrpt::img::CImage& out_img) const

Returns an image just as described in saveAsBitmapFile.

virtual void getAsMatrix(mrpt::math::CMatrixDouble& out_mat) const

Like saveAsBitmapFile(), but returns the data in matrix form (first row in the matrix is the upper (y_max) part of the map)

Like saveAsBitmapFile(), but returns the data in matrix form.

void resize(
    double new_x_min,
    double new_x_max,
    double new_y_min,
    double new_y_max,
    const TRandomFieldCell& defaultValueNewCells,
    double additionalMarginMeters = 1.0f
    )

Changes the size of the grid, maintaining previous contents.

See also:

setSize

virtual void setSize(
    const double x_min,
    const double x_max,
    const double y_min,
    const double y_max,
    const double resolution,
    const TRandomFieldCell* fill_value = nullptr
    )

Changes the size of the grid, erasing previous contents.

Parameters:

connectivity_descriptor

Optional user-supplied object that will visit all grid cells to define their connectivity with neighbors and the strength of existing edges. If present, it overrides all options in insertionOptions

See also:

resize

resize

void setCellsConnectivity(const ConnectivityDescriptor::Ptr& new_connectivity_descriptor)

Sets a custom object to define the connectivity between cells.

Must call clear() or setSize() afterwards for the changes to take place.

virtual float compute3DMatchingRatio(
    const mrpt::maps::CMetricMap* otherMap,
    const mrpt::poses::CPose3D& otherMapPose,
    const TMatchingRatioParams& params
    ) const

See docs in base class: in this class this always returns 0.

virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const

The implementation in this class just calls all the corresponding method of the contained metric maps.

virtual void saveAsMatlab3DGraph(const std::string& filName) const

Save a matlab “.m” file which represents as 3D surfaces the mean and a given confidence level for the concentration of each cell.

This method can only be called in a KF map model.

virtual void getVisualizationInto(mrpt::viz::CSetOfObjects& outObj) const

Returns a 3D object representing the map (mean)

virtual void getAs3DObject(mrpt::viz::CSetOfObjects& meanObj, mrpt::viz::CSetOfObjects& varObj) const

Returns two 3D objects representing the mean and variance maps.

TMapRepresentation getMapType()

Return the type of the random-field grid map, according to parameters passed on construction.

void insertIndividualReading(
    const double sensorReading,
    const mrpt::math::TPoint2D& point,
    const bool update_map = true,
    const bool time_invariant = true,
    const double reading_stddev = .0
    )

Direct update of the map with a reading in a given position of the map, using the appropriate method according to mapType passed in the constructor.

This is a direct way to update the map, an alternative to the generic insertObservation() method which works with mrpt::obs::CObservation objects.

virtual void predictMeasurement(
    const double x,
    const double y,
    double& out_predict_response,
    double& out_predict_response_variance,
    bool do_sensor_normalization,
    const TGridInterpolationMethod interp_method = gimNearest
    )

Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form of the expected variance).

void getMeanAndCov(mrpt::math::CVectorDouble& out_means, mrpt::math::CMatrixDouble& out_cov) const

Return the mean and covariance vector of the full Kalman filter estimate (works for all KF-based methods).

void getMeanAndSTD(mrpt::math::CVectorDouble& out_means, mrpt::math::CVectorDouble& out_STD) const

Return the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods).

void setMeanAndSTD(mrpt::math::CVectorDouble& out_means, mrpt::math::CVectorDouble& out_STD)

Load the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods).

void updateMapEstimation()

Run the method-specific procedure required to ensure that the mean & variances are up-to-date with all inserted observations.