class mrpt::poses::CPosePDFGrid
Overview
Represents a Probability Distribution Function (PDF) of a 2D pose (x, y, phi) as a discrete 3D grid over the SE(2) space.
Each cell in the grid stores a (non-negative) probability value. The grid spans a rectangular region in (x, y) and a range of heading angles phi. Cell resolutions are set independently for the spatial and angular dimensions.
Important: The constructor initializes the grid with a uniform distribution (all cells equal, summing to 1). If you intend to build a custom distribution, call uniformDistribution() or manually zero out cells before assigning probabilities, then call normalize().
See also:
CPose2D, CPosePDF, CPose2DGridTemplate
#include <mrpt/poses/CPosePDFGrid.h> class CPosePDFGrid: public mrpt::poses::CPosePDF, public mrpt::poses::CPose2DGridTemplate { public: // typedefs typedef std::shared_ptr<mrpt::poses ::CPosePDFGrid> Ptr; typedef std::shared_ptr<const mrpt::poses ::CPosePDFGrid> ConstPtr; typedef std::unique_ptr<mrpt::poses ::CPosePDFGrid> UniquePtr; typedef std::unique_ptr<const mrpt::poses ::CPosePDFGrid> ConstUniquePtr; // fields static constexpr const char* className = "mrpt::poses" "::" "CPosePDFGrid"; // construction CPosePDFGrid( double xMin = -1.0f, double xMax = 1.0f, double yMin = -1.0f, double yMax = 1.0f, double resolutionXY = 0.5f, double resolutionPhi = mrpt::DEG2RAD(180.0), double phiMin = -M_PI, double phiMax = M_PI ); // methods static constexpr auto getClassName(); static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic(); static std::shared_ptr<CObject> CreateObject(); template <typename... Args> static Ptr Create(Args&&... args); template <typename Alloc, typename... Args> static Ptr CreateAlloc( const Alloc& alloc, Args&&... args ); template <typename... Args> static UniquePtr CreateUnique(Args&&... args); virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const; virtual mrpt::rtti::CObject* clone() const; virtual void copyFrom(const CPosePDF& o); void normalize(); void uniformDistribution(); void getMean(CPose2D& mean_pose) const; virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const; virtual bool saveToTextFile(const std::string& dataFile) const; virtual void changeCoordinatesReference(const CPose3D& newReferenceBase); virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, const double minMahalanobisDistToDrop = 0); virtual void inverse(CPosePDF& o) const; void drawSingleSample(CPose2D& outPart) const; virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const; virtual void printTo(std::ostream& out) const; double getXMax() const; double getYMin() const; double getYMax() const; double getPhiMin() const; double getPhiMax() const; double getResolutionXY() const; double getResolutionPhi() const; size_t getSizeX() const; size_t getSizeY() const; size_t getSizePhi() const; };
Inherited Members
public: // typedefs typedef std::shared_ptr<CObject> Ptr; typedef std::shared_ptr<const CObject> ConstPtr; typedef std::shared_ptr<CSerializable> Ptr; typedef std::shared_ptr<const CSerializable> ConstPtr; typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t; typedef std::shared_ptr<CPosePDF> Ptr; typedef std::shared_ptr<const CPosePDF> ConstPtr; // methods static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic(); virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const; virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const; static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic(); CProbabilityDensityFunction& operator = (const CProbabilityDensityFunction&); CProbabilityDensityFunction& operator = (CProbabilityDensityFunction&&); virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const; static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic(); virtual void printTo(std::ostream& out) const = 0; virtual void copyFrom(const CPosePDF& o) = 0; virtual void bayesianFusion(const CPosePDF& p1, const CPosePDF& p2, double minMahalanobisDistToDrop = 0) = 0; virtual void inverse(CPosePDF& o) const = 0; virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0; const std::vector<T>& data() const; std::vector<T>& data(); void fill(const T& val); size_t x2idx(double x) const; size_t y2idx(double y) const; size_t phi2idx(double phi) const; double idx2x(size_t x) const; double idx2y(size_t y) const; double idx2phi(size_t phi) const; void setSize( double xMin, double xMax, double yMin, double yMax, double resolutionXY, double resolutionPhi, double phiMin = -M_PI, double phiMax = M_PI ); const T* getByPos(double x, double y, double phi) const; T* getByPos(double x, double y, double phi); size_t idx2absidx(size_t cx, size_t cy, size_t cPhi) const; std::tuple<size_t, size_t, size_t> absidx2idx(size_t absIdx) const; const T* getByIndex(size_t x, size_t y, size_t phi) const; T* getByIndex(size_t x, size_t y, size_t phi); template <class MATRIXLIKE> void getAsMatrix(double phi, MATRIXLIKE& outMat) const; double getXMin() const;
Typedefs
typedef std::shared_ptr<mrpt::poses ::CPosePDFGrid> Ptr
A type for the associated smart pointer.
Construction
CPosePDFGrid( double xMin = -1.0f, double xMax = 1.0f, double yMin = -1.0f, double yMax = 1.0f, double resolutionXY = 0.5f, double resolutionPhi = mrpt::DEG2RAD(180.0), double phiMin = -M_PI, double phiMax = M_PI )
Constructor: Initializes a uniform distribution over the given range.
After construction all cells have equal probability (uniform). Use getByPos() / getByIndex() to modify individual cells, then call normalize() before sampling or querying the distribution.
Parameters:
xMin |
Minimum x coordinate (meters). |
xMax |
Maximum x coordinate (meters). |
yMin |
Minimum y coordinate (meters). |
yMax |
Maximum y coordinate (meters). |
resolutionXY |
Spatial cell size (meters). |
resolutionPhi |
Angular cell size (radians). |
phiMin |
Minimum heading angle (radians, default -π). |
phiMax |
Maximum heading angle (radians, default +π). |
Methods
virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const
Returns information about the class of an object in runtime.
virtual mrpt::rtti::CObject* clone() const
Returns a deep copy (clone) of the object, indepently of its class.
virtual void copyFrom(const CPosePDF& o)
Copy from another PDF, translating representations if needed.
void normalize()
Normalizes the PDF so that all cells sum to 1.
void uniformDistribution()
Resets all cells to a uniform distribution (all equal, summing to 1).
void getMean(CPose2D& mean_pose) const
Computes the mean pose of the distribution.
virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const
Returns the covariance matrix and the mean of the distribution.
virtual bool saveToTextFile(const std::string& dataFile) const
Save the 3D grid to a text file as vertically concatenated matrices (one per phi level).
A companion file “<filename>_dims.txt” stores the grid dimensions.
Returns:
false on error
virtual void changeCoordinatesReference(const CPose3D& newReferenceBase)
Applies a coordinate change: this = newReferenceBase (+) this.
Useful for converting from local to global coordinates.
virtual void bayesianFusion( const CPosePDF& p1, const CPosePDF& p2, const double minMahalanobisDistToDrop = 0 )
Bayesian fusion of two densities via pointwise multiplication.
Parameters:
minMahalanobisDistToDrop |
Unused in the grid representation. |
virtual void inverse(CPosePDF& o) const
Returns the inverse PDF: NEW_PDF = (0,0,0) − THIS_PDF.
void drawSingleSample(CPose2D& outPart) const
Draws a single sample from the distribution.
The distribution must be normalized (call normalize() first).
virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const
Draws N samples from the distribution.
Each entry in outSamples is a 3-element vector [x, y, phi].
virtual void printTo(std::ostream& out) const
Write a human-readable description of this PDF to the given stream.
Derived classes must override this method.