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 CallbackEntry;
    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 const mrpt::maps::CPointsMap* asPointsMapView() const;
    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();
    output_logger_callback_id_t logRegisterCallback(output_logger_callback_t userFunc);
    bool logDeregisterCallback(output_logger_callback_t userFunc);
    bool logDeregisterCallback(output_logger_callback_id_t id);
    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, which includes the sensor noise for KF and GMRF methods).

With do_sensor_normalization, both the value and the variance are given in sensor units.

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

For mrKalmanApproximate, the cross-covariances out of the window of each cell are zero.

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.