Class CBeaconMap

Defined in File CBeaconMap.h

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

public mrpt::maps::CMetricMap

Class Documentation

class CBeaconMap : public mrpt::maps::CMetricMap 

A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).

The individual beacons are defined as

mrpt::maps::CBeacon

objects.

When invoking

CBeaconMap::insertObservation(), landmarks will be extracted and fused into the map. The only currently supported observation type is mrpt::obs::CObservationBeaconRanges

. See insertionOptions and likelihoodOptions for parameters used when creating and fusing beacon landmarks.

Use “TInsertionOptions::insertAsMonteCarlo” to select between 2 different behaviors:

Initial PDF of beacons: MonteCarlo, after convergence, pass to Gaussians; or
Initial PDF of beacons: SOG, after convergence, a single Gaussian.

Refer to the papers: []

See also

CMetricMap

Public Types

using TSequenceBeacons = std::deque<CBeacon>

using iterator = std::deque<CBeacon>::iterator

using const_iterator = std::deque<CBeacon>::const_iterator

Public Functions

CBeaconMap(): Constructor

void resize(size_t N): Resize the number of SOG modes

inline virtual std::string asString() const override: Returns a short description of the map.

inline const CBeacon &operator[](size_t i) const: Access to individual beacons

inline const CBeacon &get(size_t i) const: Access to individual beacons

inline CBeacon &operator[](size_t i): Access to individual beacons

inline CBeacon &get(size_t i): Access to individual beacons

inline iterator begin()

inline const_iterator begin() const

inline iterator end()

inline const_iterator end() const

inline void push_back(const CBeacon &m): Inserts a copy of the given mode into the SOG

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

bool saveToMATLABScript3D(const 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 occurred, true elsewere.

size_t size() const: Returns the stored landmarks count.

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

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

Perform a search for correspondences between “this” and another lansmarks map: Firsly, the landmarks’ descriptor is used to find correspondences, then inconsistent ones removed by looking at their 3D poses.

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::CBeaconMap *otherMap): Changes the reference system of the map “otherMap” and save the result in “this” map.

virtual bool isEmpty() const override: 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 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.

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

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.
”filNamePrefix”+”_covs.m”: A textual representation (see saveToTextFile)

void saveToTextFile(const std::string &fil) const

Save a text file with a row per beacon, containing this 11 elements:

X Y Z: Mean values
VX VY VZ: Variances of each dimension (C11, C22, C33)
DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
C12, C13, C23: Cross covariances

virtual void getVisualizationInto(mrpt::viz::CSetOfObjects &outObj) const override: Returns a 3D object representing the map.

const CBeacon *getBeaconByID(CBeacon::TBeaconID id) const: Returns a pointer to the beacon with the given ID, or nullptr if it does not exist.

CBeacon *getBeaconByID(CBeacon::TBeaconID id): Returns a pointer to the beacon with the given ID, or nullptr if it does not exist.

Public Members

mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOptions

mrpt::maps::CBeaconMap::TInsertionOptions insertionOptions

mrpt::maps::CBeaconMap::TInsertionOptions insertionOpts: Observations insertion options

mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOpts: Probabilistic observation likelihood options

Protected Functions

void internal_clear() override

bool internal_insertObservation(const mrpt::obs::CObservation &obs, const std::optional<const mrpt::poses::CPose3D> &robotPose) override

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

Protected Attributes

TSequenceBeacons m_beacons: The individual beacons

struct TInsertionOptions : public mrpt::config::CLoadableOptions 

This struct contains data for choosing the method by which new beacons are inserted in the map.

Public Functions

virtual void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override: Initialization of default parameters

virtual void dumpToTextStream(std::ostream &out) const override

Public Members

bool insertAsMonteCarlo = {true}

Insert a new beacon as a set of montecarlo samples (default=true), or, if false, as a sum of gaussians (see mrpt::maps::CBeacon).