class mrpt::poses::CPose3DPDFGrid
Overview
Declares a class that represents a Probability Distribution function (PDF) of a SE(3) pose (x,y,z, yaw, pitch, roll), in the form of a 6-dimensional grid of “voxels”.
See also:
CPose3D, CPose3DPDF, CPose3DGridTemplate
#include <mrpt/poses/CPose3DPDFGrid.h> class CPose3DPDFGrid: public mrpt::poses::CPose3DPDF, public mrpt::poses::CPose3DGridTemplate { public: // typedefs typedef std::shared_ptr<mrpt::poses ::CPose3DPDFGrid> Ptr; typedef std::shared_ptr<const mrpt::poses ::CPose3DPDFGrid> ConstPtr; typedef std::unique_ptr<mrpt::poses ::CPose3DPDFGrid> UniquePtr; typedef std::unique_ptr<const mrpt::poses ::CPose3DPDFGrid> ConstUniquePtr; // fields static constexpr const char* className = "mrpt::poses" "::" "CPose3DPDFGrid"; // construction CPose3DPDFGrid( const mrpt::math::TPose3D& bb_min = mrpt::math::TPose3D(-1., -1., -1., -M_PI, -.5*M_PI, -.5*M_PI), const mrpt::math::TPose3D& bb_max = mrpt::math::TPose3D(1., 1., 1., M_PI,.5*M_PI,.5*M_PI), double resolution_XYZ = 0.10, double resolution_YPR = mrpt::DEG2RAD(10.0) ); // 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; int x2idx(double x) const; int y2idx(double y) const; int z2idx(double z) const; int yaw2idx(double yaw) const; int pitch2idx(double pitch) const; int roll2idx(double roll) const; double idx2x(uint32_t cx) const; double idx2y(uint32_t cy) const; double idx2z(uint32_t cz) const; double idx2yaw(uint32_t cY) const; double idx2pitch(uint32_t cP) const; double idx2roll(uint32_t cR) const; virtual void copyFrom(const CPose3DPDF& o); void normalize(); void uniformDistribution(); void getMean(CPose3D& 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 CPose3DPDF& p1, const CPose3DPDF& p2); virtual void inverse(CPose3DPDF& o) const; void drawSingleSample(CPose3D& outPart) const; virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const; virtual void printTo(std::ostream& out) const; double* getByPos( double x, double y, double z, double yaw, double pitch, double roll ); const double* getByPos(const mrpt::math::TPose3D& p) const; double* getByPos(const mrpt::math::TPose3D& p); double* getByIndex( int cx, int cy, int cz, int cY, int cP, int cR ); mrpt::math::TPose3D getMaxBoundingBox() const; double getResolutionXYZ() const; double getResolutionAngles() const; void fill(const double& val); uint32_t getSizeX() const; uint32_t getSizeY() const; uint32_t getSizeZ() const; uint32_t getSizeYaw() const; uint32_t getSizePitch() const; uint32_t getSizeRoll() const; uint32_t getTotalVoxelCount() const; const std::vector<double>& getData() const; std::vector<double>& getData(); };
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<CPose3DPDF> Ptr; typedef std::shared_ptr<const CPose3DPDF> 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 CPose3DPDF& o) = 0; virtual void changeCoordinatesReference(const CPose3D& newReferenceBase) = 0; virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2) = 0; virtual void inverse(CPose3DPDF& o) const = 0; const std::vector<T>& data() const; std::vector<T>& data(); void setSize( const mrpt::math::TPose3D& bb_min, const mrpt::math::TPose3D& bb_max, double resolution_XYZ, double resolution_YPR ); const T* getByPos(double x, double y, double z, double yaw, double pitch, double roll) const; const T* getByIndex(int cx, int cy, int cz, int cY, int cP, int cR) const; size_t idx2absidx( size_t cx, size_t cy, size_t cz, size_t cYaw, size_t cPitch, size_t cRoll ) const; std::tuple<size_t, size_t, size_t, size_t, size_t, size_t> absidx2idx(size_t absIdx) const; template <class MATRIXLIKE> void getAsMatrix( MATRIXLIKE& outMat, const double z, const double yaw, const double pitch, const double roll ) const; mrpt::math::TPose3D getMinBoundingBox() const;
Typedefs
typedef std::shared_ptr<mrpt::poses ::CPose3DPDFGrid> Ptr
A type for the associated smart pointer.
Construction
CPose3DPDFGrid( const mrpt::math::TPose3D& bb_min = mrpt::math::TPose3D(-1., -1., -1., -M_PI, -.5*M_PI, -.5*M_PI), const mrpt::math::TPose3D& bb_max = mrpt::math::TPose3D(1., 1., 1., M_PI,.5*M_PI,.5*M_PI), double resolution_XYZ = 0.10, double resolution_YPR = mrpt::DEG2RAD(10.0) )
Constructor: Initializes a, uniform distribution over the whole given range, given by a “rectangular” (6D) bounding box.
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 CPose3DPDF& o)
Copy operator, translating if necessary (for example, between particles and gaussian representations)
void normalize()
Normalizes the PDF, such as all voxels sum the unity.
void uniformDistribution()
Assigns the same value to all the cells in the grid, so the sum 1.
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:
virtual bool saveToTextFile(const std::string& dataFile) const
Save the contents of the 3D grid in one file, as a concatenation of (X,Y) slices.
The size in X,Y,and the values for Z,yaw, pitch, roll, PHI are stored in another file named <filename>_dims.txt
Returns:
false on error
virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2)
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently only distributions of the same class can be fused! eg, gaussian with gaussian,etc)
virtual void inverse(CPose3DPDF& o) const
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void drawSingleSample(CPose3D& outPart) const
Draws a single sample from the distribution.
Precondition: voxel weights are assumed to be normalized.
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 1x6 vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum.
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.