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; 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; };
Inherited Members
public: // typedefs typedef std::shared_ptr<CObject> Ptr; typedef std::shared_ptr<const CObject> ConstPtr; typedef std::unique_ptr<CObject> UniquePtr; typedef std::unique_ptr<const CObject> ConstUniquePtr; typedef std::shared_ptr<CSerializable> Ptr; typedef std::shared_ptr<const CSerializable> ConstPtr; typedef TDATA type_value; typedef CProbabilityDensityFunction<TDATA, STATE_LEN> self_t; typedef mrpt::math::CMatrixFixed<double, STATE_LEN, STATE_LEN> cov_mat_t; typedef cov_mat_t inf_mat_t; typedef std::shared_ptr<CPose3DPDF> Ptr; typedef std::shared_ptr<const CPose3DPDF> ConstPtr; // enums enum { is_3D_val = 1, }; enum { is_PDF_val = 1, }; // fields static constexpr size_t state_length = STATE_LEN; // methods mrpt::rtti::CObject::Ptr duplicateGetSmartPtr() const; static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic(); virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const; virtual CObject* clone() const = 0; virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const; static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic(); CProbabilityDensityFunction& operator = (const CProbabilityDensityFunction&); CProbabilityDensityFunction& operator = (CProbabilityDensityFunction&&); virtual void getMean(type_value& mean_point) const = 0; virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const = 0; virtual void getCovarianceAndMean(cov_mat_t& c, TDATA& 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 bool saveToTextFile(const std::string& file) const = 0; virtual void drawSingleSample(TDATA& outPart) const = 0; virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const; double getCovarianceEntropy() const; virtual std::string asString() const = 0; virtual const mrpt::rtti::TRuntimeClassId* GetRuntimeClass() const; static const mrpt::rtti::TRuntimeClassId& GetRuntimeClassIdStatic(); virtual void printTo(std::ostream& out) const = 0; virtual std::string asString() const; 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; template <class OPENGL_SETOFOBJECTSPTR> void getAs3DObject(OPENGL_SETOFOBJECTSPTR& out_obj) const; template <class OPENGL_SETOFOBJECTSPTR> OPENGL_SETOFOBJECTSPTR getAs3DObject() const; static CPose3DPDF* createFrom2D(const CPosePDF& o); static void jacobiansPoseComposition(const CPose3D& x, const CPose3D& u, mrpt::math::CMatrixDouble66& df_dx, mrpt::math::CMatrixDouble66& df_du); static constexpr bool is_3D(); static constexpr bool is_PDF(); 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; 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; T* getByPos(double x, double y, double z, double yaw, double pitch, double roll); const T* getByPos(const mrpt::math::TPose3D& p) const; T* getByPos(const mrpt::math::TPose3D& p); const T* getByIndex(int cx, int cy, int cz, int cY, int cP, int cR) const; T* getByIndex(int cx, int cy, int cz, int cY, int cP, int cR); 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; mrpt::math::TPose3D getMaxBoundingBox() const; double getResolutionXYZ() const; double getResolutionAngles() const; void fill(const T& 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<T>& getData() const; std::vector<T>& getData();
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.