Go to the documentation of this file.
20 #include <Eigen/Dense>
30 CPoseRandomSampler::CPoseRandomSampler() =
default;
38 if (&o ==
this)
return *
this;
51 : m_pdf2D(nullptr), m_pdf3D(nullptr)
70 if (
this == &o)
return *
this;
116 std::vector<double> eigVals;
122 D = D.
asEigen().array().sqrt().matrix();
132 "Unsupported class: %s",
m_pdf2D->GetRuntimeClass()->className);
156 std::vector<double> eigVals;
164 D = D.
asEigen().array().sqrt().matrix();
174 "Unsoported class: %s",
m_pdf3D->GetRuntimeClass()->className);
200 THROW_EXCEPTION(
"No associated pdf: setPosePDF must be called first.");
224 THROW_EXCEPTION(
"No associated pdf: setPosePDF must be called first.");
246 for (
size_t i = 0; i < 3; i++)
249 for (
size_t d = 0; d < 3; d++)
275 "Unsoported class: %s",
m_pdf2D->GetRuntimeClass()->className);
296 for (
size_t i = 0; i < 6; i++)
299 for (
size_t d = 0; d < 6; d++)
328 "Unsoported class: %s",
m_pdf3D->GetRuntimeClass()->className);
347 m_pdf2D->getCovariance(cov3x3);
378 m_pdf3D->getCovariance(cov6x6);
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample,...
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose .
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
mrpt::math::CMatrixDouble66 m_fastdraw_gauss_Z6
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
double drawGaussian1D_normalized()
Generate a normalized (mean=0, std=1) normally distributed sample.
CPose3DPDF::Ptr m_pdf3D
A local copy of the PDF.
CPose2D & getSamplingMean2D(CPose2D &out_mean) const
If the object has been loaded with setPosePDF this method returns the 2D pose mean samples will be dr...
double pitch() const
Get the PITCH angle (in radians)
void getOriginalPDFCov3D(mrpt::math::CMatrixDouble66 &cov6x6) const
Retrieves the 6x6 covariance of the original PDF in .
double phi() const
Get the phi angle of the 2D pose (in radians)
void setPosePDF(const CPosePDF &pdf)
This method must be called to select the PDF from which to draw samples.
double x() const
Common members of all points & poses classes.
CPose2D m_fastdraw_gauss_M_2D
virtual CObject * clone() const =0
Returns a deep copy (clone) of the object, indepently of its class.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
bool isPrepared() const
Return true if samples can be generated, which only requires a previous call to setPosePDF.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
CPosePDF::Ptr m_pdf2D
A local copy of the PDF.
CPose2D & drawSample(CPose2D &p) const
Generate a new sample from the selected PDF.
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
bool eig_symmetric(Derived &eVecs, std::vector< Scalar > &eVals, bool sorted=true) const
Read: eig()
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void matProductOf_AB(const Derived &A, const Derived &B)
this = A*B, with A & B of the same type of this.
CMatrixDynamic< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!...
CPoseRandomSampler & operator=(const CPoseRandomSampler &o)
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!...
double roll() const
Get the ROLL angle (in radians)
void setDiagonal(const std::size_t N, const Scalar value)
Resize to NxN, set all entries to zero, except the main diagonal which is set to value
void do_sample_3D(CPose3D &p) const
Used internally: sample from m_pdf3D.
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,...
An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose pro...
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
void getOriginalPDFCov2D(mrpt::math::CMatrixDouble33 &cov3x3) const
Retrieves the 3x3 covariance of the original PDF in .
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double yaw() const
Get the YAW angle (in radians)
This base provides a set of functions for maths stuff.
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
mrpt::math::CMatrixDouble33 m_fastdraw_gauss_Z3
A namespace of pseudo-random numbers generators of diferent distributions.
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
CPose3D & getSamplingMean3D(CPose3D &out_mean) const
If the object has been loaded with setPosePDF this method returns the 3D pose mean samples will be dr...
void do_sample_2D(CPose2D &p) const
Used internally: sample from m_pdf2D.
void clear()
Clear internal pdf.
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
CPose3D m_fastdraw_gauss_M_3D
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 15:49:54 UTC 2020 | |