class mrpt::maps::CBeacon

Overview

The class for storing individual “beacon landmarks” under a variety of 3D position PDF distributions.

This class is used for storage within the class CBeaconMap. The class implements the same methods than the interface “CPointPDF”, and invoking them actually becomes a mapping into the methods of the current PDF representation of the beacon, selectable by means of “m_typePDF”

See also:

CBeaconMap, CPointPDFSOG

#include <mrpt/maps/CBeacon.h>

class CBeacon: public mrpt::poses::CPointPDF
{
public:
    // typedefs

    typedef int64_t TBeaconID;
    typedef CPoint3D type_value;
    typedef mrpt::math::CMatrixFixed<double, STATE_LEN, STATE_LEN> cov_mat_t;
    typedef cov_mat_t inf_mat_t;

    // enums

    enum TTypePDF;

    // fields

    TTypePDF m_typePDF {pdfGauss};
    mrpt::poses::CPointPDFParticles m_locationMC {1};
    mrpt::poses::CPointPDFGaussian m_locationGauss;
    mrpt::poses::CPointPDFSOG m_locationSOG {1};
    TBeaconID m_ID = mrpt::obs::INVALID_BEACON_ID;
    static constexpr size_t state_length;

    // methods

    void getMean(mrpt::poses::CPoint3D& mean_point) const;
    virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const;
    virtual void copyFrom(const mrpt::poses::CPointPDF& o);
    virtual bool saveToTextFile(const std::string& file) const;
    virtual void changeCoordinatesReference(const mrpt::poses::CPose3D& newReferenceBase);
    void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const;
    void getAsMatlabDrawCommands(std::vector<std::string>& out_Str) const;
    void drawSingleSample(mrpt::poses::CPoint3D& outSample) const;
    void bayesianFusion(const CPointPDF& p1, const CPointPDF& p2, const double minMahalanobisDistToDrop = 0);

    void generateObservationModelDistribution(
        float sensedRange,
        mrpt::poses::CPointPDFSOG& outPDF,
        const CBeaconMap* myBeaconMap,
        const mrpt::poses::CPoint3D& sensorPntOnRobot,
        const mrpt::poses::CPoint3D& centerPoint = mrpt::poses::CPoint3D(0, 0, 0),
        float maxDistanceFromCenter = 0
        ) const;

    virtual void bayesianFusion(
        const CPointPDF& p1,
        const CPointPDF& p2,
        const double minMahalanobisDistToDrop = 0
        ) = 0;

    template <class OPENGL_SETOFOBJECTSPTR>
    void getAs3DObject(OPENGL_SETOFOBJECTSPTR& out_obj) const;

    template <class OPENGL_SETOFOBJECTSPTR, class OPENGL_SETOFOBJECTS>
    OPENGL_SETOFOBJECTSPTR getAs3DObject() const;

    virtual void getMean(type_value& mean_point) const = 0;
    virtual void getCovarianceAndMean(cov_mat_t& c, CPoint3D& mean) const;
    void getCovarianceDynAndMean(mrpt::math::CMatrixDouble& cov, type_value& mean_point) const;
    type_value getMeanVal() const;
    void getCovariance(mrpt::math::CMatrixDouble& cov) const;
    void getCovariance(cov_mat_t& cov) const;
    cov_mat_t getCovariance() const;
    virtual bool isInfType() const;
    virtual void getInformationMatrix(inf_mat_t& inf) const;
    virtual void drawSingleSample(CPoint3D& outPart) const = 0;
    virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const;
    double getCovarianceEntropy() const;

    static void generateRingSOG(
        float sensedRange,
        mrpt::poses::CPointPDFSOG& outPDF,
        const CBeaconMap* myBeaconMap,
        const mrpt::poses::CPoint3D& sensorPnt,
        const mrpt::math::CMatrixDouble33* covarianceCompositionToAdd = nullptr,
        bool clearPreviousContentsOutPDF = true,
        const mrpt::poses::CPoint3D& centerPoint = mrpt::poses::CPoint3D(0, 0, 0),
        float maxDistanceFromCenter = 0
        );
};

Inherited Members

public:
    // typedefs

    typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t;

    // methods

    virtual void copyFrom(const CPointPDF& o) = 0;
    virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0;

Typedefs

typedef int64_t TBeaconID

The type for the IDs of landmarks.

typedef CPoint3D type_value

The type of the state the PDF represents.

typedef mrpt::math::CMatrixFixed<double, STATE_LEN, STATE_LEN> cov_mat_t

Covariance matrix type.

typedef cov_mat_t inf_mat_t

Information matrix type.

Fields

TTypePDF m_typePDF {pdfGauss}

Which one of the different 3D point PDF is currently used in this object: montecarlo, gaussian, or a sum of gaussians.

See also:

m_location

mrpt::poses::CPointPDFParticles m_locationMC {1}

The individual PDF, if m_typePDF=pdfMonteCarlo (publicly accessible for ease of use, but the CPointPDF interface is also implemented in CBeacon).

mrpt::poses::CPointPDFGaussian m_locationGauss

The individual PDF, if m_typePDF=pdfGauss (publicly accessible for ease of use, but the CPointPDF interface is also implemented in CBeacon).

mrpt::poses::CPointPDFSOG m_locationSOG {1}

The individual PDF, if m_typePDF=pdfSOG (publicly accessible for ease of use, but the CPointPDF interface is also implemented in CBeacon).

TBeaconID m_ID = mrpt::obs::INVALID_BEACON_ID

An ID for the landmark (see details next…) This ID was introduced in the version 3 of this class (21/NOV/2006), and its aim is to provide a way for easily establishing correspondences between landmarks detected in sequential image frames.

Thus, the management of this field should be:

  • In ‘servers’ (classes/modules/… that detect landmarks from images): A different ID must be assigned to every landmark (e.g. a sequential counter), BUT only in the case of being sure of the correspondence of one landmark with another one in the past (e.g. tracking).

  • In ‘clients’: This field can be ignored, but if it is used, the advantage is solving the correspondence between landmarks detected in consequentive instants of time: Two landmarks with the same ID correspond to the same physical feature, BUT it should not be expected the inverse to be always true.

Note that this field is never fill out automatically, it must be set by the programmer if used.

static constexpr size_t state_length

The length of the variable, for example, 3 for a 3D point, 6 for a 3D pose (x y z yaw pitch roll).

Methods

virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const

Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.

See also:

getMean, getInformationMatrix

virtual void copyFrom(const mrpt::poses::CPointPDF& o)

Copy operator, translating if necessary (for example, between particles and gaussian representations)

virtual bool saveToTextFile(const std::string& file) const

Save PDF’s particles to a text file.

See derived classes for more information about the format of generated files

virtual void changeCoordinatesReference(const mrpt::poses::CPose3D& newReferenceBase)

this = p (+) this.

This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which “to project” the current pdf. Result PDF substituted the currently stored one in the object.

void getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const

Saves a 3D representation of the beacon into a given OpenGL scene

void getAsMatlabDrawCommands(std::vector<std::string>& out_Str) const

Gets a set of MATLAB commands which draw the current state of the beacon:

void drawSingleSample(mrpt::poses::CPoint3D& outSample) const

Draw a sample from the pdf.

void bayesianFusion(
    const CPointPDF& p1,
    const CPointPDF& p2,
    const double minMahalanobisDistToDrop = 0
    )

Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)

Parameters:

p1

The first distribution to fuse

p2

The second distribution to fuse

minMahalanobisDistToDrop

If set to different of 0, the result of very separate Gaussian modes (that will result in negligible components) in SOGs will be dropped to reduce the number of modes in the output.

 
void generateObservationModelDistribution(
    float sensedRange,
    mrpt::poses::CPointPDFSOG& outPDF,
    const CBeaconMap* myBeaconMap,
    const mrpt::poses::CPoint3D& sensorPntOnRobot,
    const mrpt::poses::CPoint3D& centerPoint = mrpt::poses::CPoint3D(0, 0, 0),
    float maxDistanceFromCenter = 0
    ) const

Compute the observation model p(z_t|x_t) for a given observation (range value), and return it as an approximate SOG.

Note that if the beacon is a SOG itself, the number of gaussian modes will be square. As a speed-up, if a “center point”+”maxDistanceFromCenter” is supplied (maxDistanceFromCenter!=0), those modes farther than this sphere will be discarded. Parameters such as the stdSigma of the sensor are gathered from “myBeaconMap” The result is one “ring” for each Gaussian mode that represent the beacon position in this object. The position of the sensor on the robot is used to shift the resulting densities such as they represent the position of the robot, not the sensor.

See also:

CBeaconMap::insertionOptions, generateRingSOG

virtual void bayesianFusion(
    const CPointPDF& p1,
    const CPointPDF& p2,
    const double minMahalanobisDistToDrop = 0
    ) = 0

Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)

Parameters:

p1

The first distribution to fuse

p2

The second distribution to fuse

minMahalanobisDistToDrop

If set to different of 0, the result of very separate Gaussian modes (that will result in negligible components) in SOGs will be dropped to reduce the number of modes in the output.

 
template <class OPENGL_SETOFOBJECTSPTR>
void getAs3DObject(OPENGL_SETOFOBJECTSPTR& out_obj) const

Returns a 3D representation of this PDF (it doesn’t clear the current contents of out_obj, but append new OpenGL objects to that list)

Needs the mrpt-opengl library, and using mrpt::opengl::CSetOfObjects::Ptr as template argument.

By default, ellipsoids for the confidence intervals of “q=3” are drawn; for more mathematical details, see CGeneralizedEllipsoidTemplate::setQuantiles()

template <class OPENGL_SETOFOBJECTSPTR, class OPENGL_SETOFOBJECTS>
OPENGL_SETOFOBJECTSPTR getAs3DObject() const

Returns a 3D representation of this PDF.

Needs the mrpt-opengl library, and using mrpt::opengl::CSetOfObjects::Ptr as template argument.

virtual void getMean(type_value& mean_point) const = 0

Returns the mean, or mathematical expectation of the probability density distribution (PDF).

See also:

getCovarianceAndMean, getInformationMatrix

virtual void getCovarianceAndMean(cov_mat_t& c, CPoint3D& mean) const

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

void getCovarianceDynAndMean(mrpt::math::CMatrixDouble& cov, type_value& mean_point) const

Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.

See also:

getMean, getInformationMatrix

type_value getMeanVal() const

Returns the mean, or mathematical expectation of the probability density distribution (PDF).

See also:

getCovariance, getInformationMatrix

void getCovariance(mrpt::math::CMatrixDouble& cov) const

Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)

See also:

getMean, getCovarianceAndMean, getInformationMatrix

void getCovariance(cov_mat_t& cov) const

Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)

See also:

getMean, getCovarianceAndMean, getInformationMatrix

cov_mat_t getCovariance() const

Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)

See also:

getMean, getInformationMatrix

virtual bool isInfType() const

Returns whether the class instance holds the uncertainty in covariance or information form.

By default this is going to be covariance form. *Inf classes (e.g. CPosePDFGaussianInf) store it in information form.

See also:

mrpt::traits::is_inf_type

virtual void getInformationMatrix(inf_mat_t& inf) const

Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix) Unless reimplemented in derived classes, this method first reads the covariance, then invert it.

See also:

getMean, getCovarianceAndMean

virtual void drawSingleSample(CPoint3D& outPart) const = 0

Draws a single sample from the distribution.

virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const

Draws a number of samples from the distribution, and saves as a list of 1xSTATE_LEN vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum.

This base method just call N times to drawSingleSample, but derived classes should implemented optimized method for each particular PDF.

double getCovarianceEntropy() const

Compute the entropy of the estimated covariance matrix.

See also:

http://en.wikipedia.org/wiki/Multivariate_normal_distribution#Entropy

static void generateRingSOG(
    float sensedRange,
    mrpt::poses::CPointPDFSOG& outPDF,
    const CBeaconMap* myBeaconMap,
    const mrpt::poses::CPoint3D& sensorPnt,
    const mrpt::math::CMatrixDouble33* covarianceCompositionToAdd = nullptr,
    bool clearPreviousContentsOutPDF = true,
    const mrpt::poses::CPoint3D& centerPoint = mrpt::poses::CPoint3D(0, 0, 0),
    float maxDistanceFromCenter = 0
    )

This static method returns a SOG with ring-shape (or as a 3D sphere) that can be used to initialize a beacon if observed the first time.

sensorPnt is the center of the ring/sphere, i.e. the absolute position of the range sensor. If clearPreviousContentsOutPDF=false, the SOG modes will be added to the current contents of outPDF If the 3x3 matrix covarianceCompositionToAdd is provided, it will be add to every Gaussian (to model the composition of uncertainty).

See also:

generateObservationModelDistribution