class mrpt::poses::CPose3DPDFSOG
Overview
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose \(p(\mathbf{x}) = [x ~ y ~ z ~ yaw ~ pitch ~ roll]^t\).
This class implements that PDF as the following multi-modal Gaussian distribution:
\(p(\mathbf{x}) = \sum\limits_{i=1}^N \omega^i \mathcal{N}( \mathbf{x} ; \bar{\mathbf{x}}^i, \mathbf{\Sigma}^i )\)
Where the number of modes N is the size of CPose3DPDFSOG::m_modes. Angles are always in radians.
See mrpt::poses::CPose3DPDF for more details.
See also:
#include <mrpt/poses/CPose3DPDFSOG.h> class CPose3DPDFSOG: public mrpt::poses::CPose3DPDF { public: // typedefs typedef std::shared_ptr<mrpt::poses ::CPose3DPDFSOG> Ptr; typedef std::shared_ptr<const mrpt::poses ::CPose3DPDFSOG> ConstPtr; typedef std::unique_ptr<mrpt::poses ::CPose3DPDFSOG> UniquePtr; typedef std::unique_ptr<const mrpt::poses ::CPose3DPDFSOG> ConstUniquePtr; typedef std::vector<TGaussianMode, mrpt::aligned_allocator_cpp11<TGaussianMode>> TModesList; typedef TModesList::const_iterator const_iterator; typedef TModesList::iterator iterator; // enums enum { is_3D_val = 1, }; enum { is_PDF_val = 1, }; // structs struct TGaussianMode; // fields static constexpr const char* className = "mrpt::poses" "::" "CPose3DPDFSOG"; // construction CPose3DPDFSOG(size_t nModes = 1); // 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; static constexpr bool is_3D(); static constexpr bool is_PDF(); void clear(); void resize(size_t N); size_t size() const; bool empty() const; iterator begin(); iterator end(); const_iterator begin() const; const_iterator end() const; void getMean(CPose3D& mean_pose) const; CPose3D meanVal() const; virtual std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const; void normalizeWeights(); void getMostLikelyMode(CPose3DPDFGaussian& outVal) const; CPose3DPDFGaussian getMostLikelyMode() const; virtual void copyFrom(const CPose3DPDF& o); virtual bool saveToTextFile(const std::string& file) const; virtual void changeCoordinatesReference(const CPose3D& newReferenceBase); virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2); void drawSingleSample(CPose3D& outPart) const; virtual void drawManySamples(size_t N, std::vector<mrpt::math::CVectorDouble>& outSamples) const; virtual void inverse(CPose3DPDF& o) const; void appendFrom(const CPose3DPDFSOG& o); 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::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;
Typedefs
typedef std::shared_ptr<mrpt::poses ::CPose3DPDFSOG> Ptr
A type for the associated smart pointer.
Construction
CPose3DPDFSOG(size_t nModes = 1)
Default constructor.
Parameters:
nModes |
The initial size of CPose3DPDFSOG::m_modes |
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.
void clear()
Clear all the gaussian modes.
void resize(size_t N)
Set the number of SOG modes.
size_t size() const
Return the number of Gaussian modes.
bool empty() const
Return whether there is any Gaussian mode.
CPose3D meanVal() const
Returns the mean pose by value (convenience non-virtual overload).
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:
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
void getMostLikelyMode(CPose3DPDFGaussian& outVal) const
Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG).
Deprecated Use getMostLikelyMode() returning by value instead.
CPose3DPDFGaussian getMostLikelyMode() const
Returns the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG).
virtual void copyFrom(const CPose3DPDF& o)
Copy operator, translating if necessary (for example, between particles and gaussian representations)
virtual bool saveToTextFile(const std::string& file) const
Save the density to a text file, with the following format: There is one row per Gaussian “mode”, and each row contains 10 elements:
w (The linear weight)
x_mean (gaussian mean value)
y_mean (gaussian mean value)
x_mean (gaussian mean value)
yaw_mean (gaussian mean value, in radians)
pitch_mean (gaussian mean value, in radians)
roll_mean (gaussian mean value, in radians)
C11,C22,C33,C44,C55,C66 (Covariance elements)
C12,C13,C14,C15,C16 (Covariance elements)
C23,C24,C25,C25 (Covariance elements)
C34,C35,C36 (Covariance elements)
C45,C46 (Covariance elements)
C56 (Covariance elements)
virtual void changeCoordinatesReference(const 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.
virtual void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2)
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1 must be a mrpt::poses::CPose3DPDFSOG object and p2 a mrpt::poses::CPose3DPDFSOG object)
void drawSingleSample(CPose3D& outPart) const
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 1x6 vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum.
virtual void inverse(CPose3DPDF& o) const
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void appendFrom(const CPose3DPDFSOG& o)
Append the Gaussian modes from “o” to the current set of modes of “this” density.
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.