class mrpt::maps::CLandmarksMap

A class for storing a map of 3D probabilistic landmarks.

Currently these types of landmarks are defined: (see mrpt::maps::CLandmark)

For “visual landmarks” from images: features with associated descriptors.
For laser scanners: each of the range measuremnts, as “occupancy” landmarks.
For grid maps: “Panoramic descriptor” feature points.
For range-only localization and SLAM: Beacons. It is also supported the simulation of expected beacon-to-sensor readings, observation likelihood,…

How to load landmarks from observations:

When invoking CLandmarksMap::insertObservation(), the values in CLandmarksMap::insertionOptions will determinate the kind of landmarks that will be extracted and fused into the map. Supported feature extraction processes are listed next:

CObservationImage vlSIFT 1) A SIFT feature is created for each SIFT detected in the image, CObservationStereoImages vlSIFT Each image is separately procesed by the method for CObservationImage observations CObservationStereoImages vlColor TODO… CObservation2DRangeScan glOccupancy A landmark is added for each range in the scan, with its appropiate covariance matrix derived from the jacobians matrixes. ======================== =========== ==========================================================================================================================

See also:

CMetricMap

#include <mrpt/maps/CLandmarksMap.h>

class CLandmarksMap: public mrpt::maps::CMetricMap
{
public:
    // typedefs

    typedef mrpt::maps::CLandmark landmark_type;
    typedef std::pair<mrpt::math::TPoint3D, unsigned int> TPairIdBeacon;

    // structs

    struct TCustomSequenceLandmarks;
    struct TFuseOptions;
    struct TInsertionOptions;
    struct TInsertionResults;
    struct TLikelihoodOptions;

    //
fields

    static mrpt::img::TColorf COLOR_LANDMARKS_IN_3DSCENES;
    static std::map<std::pair<mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID>, double> _mEDD;
    static mrpt::maps::CLandmark::TLandmarkID _mapMaxID;
    static bool _maxIDUpdated = false;
    struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks;
    mrpt::maps::CLandmarksMap::TInsertionOptions insertionOptions;
    mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOptions;
    struct mrpt::maps::CLandmarksMap::TInsertionResults insertionResults;
    struct mrpt::maps::CLandmarksMap::TFuseOptions fuseOptions;
    std::deque<TPairIdBeacon> initialBeacons;
    mrpt::maps::CLandmarksMap::TInsertionOptions insertionOpts;
    mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOpts;

    // construction

    CLandmarksMap();

    //
methods

    virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation& obs, const mrpt::poses::CPose3D& takenFrom) const;
    virtual std::string asString() const;
    mrpt::maps::CLandmark::TLandmarkID getMapMaxID();
    virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap* otherMap, const mrpt::poses::CPose3D& otherMapPose, const TMatchingRatioParams& params) const;
    bool saveToTextFile(std::string file);
    bool saveToMATLABScript2D(std::string file, const char* style = "b", float stdCount = 2.0f);
    bool saveToMATLABScript3D(std::string file, const char* style = "b", float confInterval = 0.95f) const;
    size_t size() const;
    double computeLikelihood_RSLC_2007(const CLandmarksMap* s, const mrpt::poses::CPose2D& sensorPose) const;
    void loadSiftFeaturesFromImageObservation(const mrpt::obs::CObservationImage& obs, const mrpt::vision::CFeatureExtraction::TOptions& feat_options = mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT));
    void loadSiftFeaturesFromStereoImageObservation(const mrpt::obs::CObservationStereoImages& obs, mrpt::maps::CLandmark::TLandmarkID fID, const mrpt::vision::CFeatureExtraction::TOptions& feat_options = mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT));

    void loadOccupancyFeaturesFrom2DRangeScan(
        const mrpt::obs::CObservation2DRangeScan& obs,
        const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt,
        unsigned int downSampleFactor = 1
        );

    void computeMatchingWith2D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose2D& otherMapPose,
        float maxDistForCorrespondence,
        float maxAngularDistForCorrespondence,
        const mrpt::poses::CPose2D& angularDistPivotPoint,
        mrpt::tfest::TMatchingPairList& correspondences,
        float& correspondencesRatio,
        float* sumSqrDist = nullptr,
        bool onlyKeepTheClosest = false,
        bool onlyUniqueRobust = false
        ) const;

    void computeMatchingWith3DLandmarks(
        const mrpt::maps::CLandmarksMap* otherMap,
        mrpt::tfest::TMatchingPairList& correspondences,
        float& correspondencesRatio,
        std::vector<bool>& otherCorrespondences
        ) const;

    void changeCoordinatesReference(const mrpt::poses::CPose3D& newOrg);
    void changeCoordinatesReference(const mrpt::poses::CPose3D& newOrg, const mrpt::maps::CLandmarksMap* otherMap);
    void fuseWith(CLandmarksMap& other, bool justInsertAllOfThem = false);

    double computeLikelihood_SIFT_LandmarkMap(
        CLandmarksMap* map,
        mrpt::tfest::TMatchingPairList* correspondences = nullptr,
        std::vector<bool>* otherCorrespondences = nullptr
        ) const;

    virtual bool isEmpty() const;

    void simulateBeaconReadings(
        const mrpt::poses::CPose3D& in_robotPose,
        const mrpt::poses::CPoint3D& in_sensorLocationOnRobot,
        mrpt::obs::CObservationBeaconRanges& out_Observations
        ) const;

    void simulateRangeBearingReadings(
        const mrpt::poses::CPose3D& robotPose,
        const mrpt::poses::CPose3D& sensorLocationOnRobot,
        mrpt::obs::CObservationBearingRange& observations,
        bool sensorDetectsIDs = true,
        const float stdRange = 0.01f,
        const float stdYaw = mrpt::DEG2RAD(0.1f),
        const float stdPitch = mrpt::DEG2RAD(0.1f),
        std::vector<size_t>* real_associations = nullptr,
        const double spurious_count_mean = 0,
        const double spurious_count_std = 0
        ) const;

    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const;
    virtual void getVisualizationInto(mrpt::opengl::CSetOfObjects& outObj) const;
    virtual void auxParticleFilterCleanUp();
};

Inherited Members

public:
    //
methods

    virtual bool isEmpty() const = 0;
    virtual void saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const = 0;
    virtual std::string asString() const = 0;
    virtual void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const = 0;

Fields

static mrpt::img::TColorf COLOR_LANDMARKS_IN_3DSCENES

The color of landmark ellipsoids in CLandmarksMap::getAs3DObject.

static std::map<std::pair<mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID>, double> _mEDD

Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks.

std::deque<TPairIdBeacon> initialBeacons

Initial contents of the map, especified by a set of 3D Beacons with associated IDs.

Methods

virtual double internal_computeObservationLikelihood(
    const mrpt::obs::CObservation& obs,
    const mrpt::poses::CPose3D& takenFrom
    ) const

Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the world being modeled with this map.

In the current implementation, this method behaves in a different way according to the nature of the observation’s class:

“mrpt::obs::CObservation2DRangeScan”: This calls “computeLikelihood_RSLC_2007”.

“mrpt::obs::CObservationStereoImages”: This calls “computeLikelihood_SIFT_LandmarkMap”. ======================== =========== ==========================================================================================================================

CObservationImage	vlSIFT	A SIFT feature is created for each SIFT detected in the image,
CObservationStereoImages	vlSIFT	Each image is separately procesed by the method for CObservationImage observations
CObservationStereoImages	vlColor	TODO…
CObservation2DRangeScan	glOccupancy	A landmark is added for each range in the scan, with its appropiate covariance matrix derived from the jacobians matrixes.

Parameters:

takenFrom	The robot’s pose the observation is supposed to be taken from.
obs	The observation.

Returns:

This method returns a likelihood value > 0.

See also:

Used in particle filter algorithms, see: CMultiMetricMapPDF::update

virtual std::string asString() const

Returns a short description of the map.

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

Computes the ratio in [0,1] of correspondences between “this” and the “otherMap” map, whose 6D pose relative to “this” is “otherMapPose” In the case of a multi-metric map, this returns the average between the maps.

This method always return 0 for grid maps.

Parameters:

otherMap	[IN] The other map to compute the matching with.
otherMapPose	[IN] The 6D pose of the other map as seen from “this”.
params	[IN] Matching parameters

Returns:

The matching ratio [0,1]

See also:

determineMatching2D

bool saveToTextFile(std::string file)

Save to a text file.

In line “i” there are the (x,y,z) mean values of the i’th landmark + type of landmark + # times seen + timestamp + RGB/descriptor + ID

Returns false if any error occured, true elsewere.

bool saveToMATLABScript2D(
    std::string file,
    const char* style = "b",
    float stdCount = 2.0f
    )

Save to a MATLAB script which displays 2D error ellipses for the map (top-view, projection on the XY plane).

Parameters:

file	The name of the file to save the script to.
style	The MATLAB-like string for the style of the lines (see ‘help plot’ in MATLAB for possibilities)
stdCount	The ellipsoids will be drawn from the center to “stdCount” times the “standard deviations”. (default is 2std = 95% confidence intervals)

Returns:

Returns false if any error occured, true elsewere.

bool saveToMATLABScript3D(
    std::string file,
    const char* style = "b",
    float confInterval = 0.95f
    ) const

Save to a MATLAB script which displays 3D error ellipses for the map.

Parameters:

file	The name of the file to save the script to.
style	The MATLAB-like string for the style of the lines (see ‘help plot’ in MATLAB for possibilities)
stdCount	The ellipsoids will be drawn from the center to a given confidence interval in [0,1], e.g. 2 sigmas=0.95 (default is 2std = 0.95 confidence intervals)

Returns:

Returns false if any error occured, true elsewere.

size_t size() const

Returns the stored landmarks count.

double computeLikelihood_RSLC_2007(const CLandmarksMap* s, const mrpt::poses::CPose2D& sensorPose) const

Computes the (logarithmic) likelihood function for a sensed observation “o” according to “this” map.

This is the implementation of the algorithm reported in the paper: J.L. Blanco, J. Gonzalez, and J.A. Fernandez-Madrigal, “A Consensus-based Approach for Estimating the Observation Likelihood of Accurate Range Sensors”, in IEEE International Conference on Robotics and Automation (ICRA), Rome (Italy), Apr 10-14, 2007

void loadSiftFeaturesFromImageObservation(
    const mrpt::obs::CObservationImage& obs,
    const mrpt::vision::CFeatureExtraction::TOptions& feat_options = mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT)
    )

Loads into this landmarks map the SIFT features extracted from an image observation (Previous contents of map will be erased) The robot is assumed to be at the origin of the map.

Some options may be applicable from “insertionOptions” (insertionOptions.SIFTsLoadDistanceOfTheMean)

Parameters:

feat_options

Optionally, you can pass here parameters for changing the default SIFT detector settings.

void loadSiftFeaturesFromStereoImageObservation(
    const mrpt::obs::CObservationStereoImages& obs,
    mrpt::maps::CLandmark::TLandmarkID fID,
    const mrpt::vision::CFeatureExtraction::TOptions& feat_options = mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT)
    )

Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of stereo-image (Previous contents of map will be erased) The robot is assumed to be at the origin of the map.

Some options may be applicable from “insertionOptions”

Parameters:

feat_options

Optionally, you can pass here parameters for changing the default SIFT detector settings.

void loadOccupancyFeaturesFrom2DRangeScan(
    const mrpt::obs::CObservation2DRangeScan& obs,
    const std::optional<const mrpt::poses::CPose3D>& robotPose = std::nullopt,
    unsigned int downSampleFactor = 1
    )

Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous contents of map will be erased)

Parameters:

obs	The observation
robotPose	The robot pose in the map (Default = the origin) Some options may be applicable from “insertionOptions”

void computeMatchingWith3DLandmarks(
    const mrpt::maps::CLandmarksMap* otherMap,
    mrpt::tfest::TMatchingPairList& correspondences,
    float& correspondencesRatio,
    std::vector<bool>& otherCorrespondences
    ) const

Perform a search for correspondences between “this” and another lansmarks map: In this class, the matching is established solely based on the landmarks’ IDs.

Parameters:

otherMap	[IN] The other map.
correspondences	[OUT] The matched pairs between maps.
correspondencesRatio	[OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap
otherCorrespondences	[OUT] Will be returned with a vector containing “true” for the indexes of the other map’s landmarks with a correspondence.

void changeCoordinatesReference(const mrpt::poses::CPose3D& newOrg)

Changes the reference system of the map to a given 3D pose.

void changeCoordinatesReference(
    const mrpt::poses::CPose3D& newOrg,
    const mrpt::maps::CLandmarksMap* otherMap
    )

Changes the reference system of the map “otherMap” and save the result in “this” map.

void fuseWith(CLandmarksMap& other, bool justInsertAllOfThem = false)

Fuses the contents of another map with this one, updating “this” object with the result.

This process involves fusing corresponding landmarks, then adding the new ones.

Parameters:

other	The other landmarkmap, whose landmarks will be inserted into “this”
justInsertAllOfThem	If set to “true”, all the landmarks in “other” will be inserted into “this” without checking for possible correspondences (may appear duplicates ones, etc…)

double computeLikelihood_SIFT_LandmarkMap(
    CLandmarksMap* map,
    mrpt::tfest::TMatchingPairList* correspondences = nullptr,
    std::vector<bool>* otherCorrespondences = nullptr
    ) const

Returns the (logarithmic) likelihood of a set of landmarks “map” given “this” map.

See paper: JJAA 2006

virtual bool isEmpty() const

Returns true if the map is empty/no observation has been inserted.

void simulateBeaconReadings(
    const mrpt::poses::CPose3D& in_robotPose,
    const mrpt::poses::CPoint3D& in_sensorLocationOnRobot,
    mrpt::obs::CObservationBeaconRanges& out_Observations
    ) const

Simulates a noisy reading toward each of the beacons in the landmarks map, if any.

Parameters:

in_robotPose	This robot pose is used to simulate the ranges to each beacon.
in_sensorLocationOnRobot	The 3D position of the sensor on the robot
out_Observations	The results will be stored here. NOTICE that the fields “CObservationBeaconRanges::minSensorDistance”,”CObservationBeaconRanges::maxSensorDistance” and “CObservationBeaconRanges::stdError” MUST BE FILLED OUT before calling this function. An observation will be generated for each beacon in the map, but notice that some of them may be missed if out of the sensor maximum range.

void simulateRangeBearingReadings(
    const mrpt::poses::CPose3D& robotPose,
    const mrpt::poses::CPose3D& sensorLocationOnRobot,
    mrpt::obs::CObservationBearingRange& observations,
    bool sensorDetectsIDs = true,
    const float stdRange = 0.01f,
    const float stdYaw = mrpt::DEG2RAD(0.1f),
    const float stdPitch = mrpt::DEG2RAD(0.1f),
    std::vector<size_t>* real_associations = nullptr,
    const double spurious_count_mean = 0,
    const double spurious_count_std = 0
    ) const

Simulates a noisy bearing-range observation of all the beacons (landamrks with type glBeacon) in the landmarks map, if any.

The fields “CObservationBearingRange::fieldOfView_*”,”CObservationBearingRange::maxSensorDistance” and “CObservationBearingRange::minSensorDistance” MUST BE FILLED OUT before calling this function.

At output, the observation will have CObservationBearingRange::validCovariances set to “false” and the 3 sensor_std_* members correctly set to their values. An observation will be generated for each beacon in the map, but notice that some of them may be missed if out of the sensor maximum range or field of view-

Parameters:

robotPose	The robot pose.
sensorLocationOnRobot	The 3D position of the sensor on the robot
observations	The results will be stored here.
sensorDetectsIDs	If this is set to false, all the landmarks will be sensed with INVALID_LANDMARK_ID as ID.
stdRange	The sigma of the sensor noise in range (meters).
stdYaw	The sigma of the sensor noise in yaw (radians).
stdPitch	The sigma of the sensor noise in pitch (radians).
real_associations	If it’s not a nullptr pointer, this will contain at the return the real indices of the landmarks in the map in the same order than they appear in out_Observations. Useful when sensorDetectsIDs=false. Spurious readings are assigned a std::string::npos (=-1) index.
spurious_count_mean	The mean number of spurious measurements (uniformly distributed in range & angle) to generate. The number of spurious is generated by rounding a random Gaussin number. If both this mean and the std are zero (the default) no spurious readings are generated.
spurious_count_std	Read spurious_count_mean above.

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

This virtual method saves the map to a file “filNamePrefix”+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).

In the case of this class, these files are generated:

“filNamePrefix”+”_3D.m”: A script for MATLAB for drawing landmarks as 3D ellipses.
“filNamePrefix”+”_3D.3DScene”: A 3D scene with a “ground plane grid” and the set of ellipsoids in 3D.

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

Returns a 3D object representing the map.

See also:

COLOR_LANDMARKS_IN_3DSCENES

virtual void auxParticleFilterCleanUp()

This method is called at the end of each “prediction-update-map insertion” cycle within “mrpt::slam::CMetricMapBuilderRBPF::processActionObservation”.

This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.