9 #ifndef CPosePDFGaussian_H 10 #define CPosePDFGaussian_H 20 class CPoint2DPDFGaussian;
40 void assureSymmetry();
88 void copyFrom(
const CPosePDF &o);
99 void changeCoordinatesReference(
const CPose3D &newReferenceBase );
104 void changeCoordinatesReference(
const CPose2D &newReferenceBase );
108 void rotateCov(
const double ang);
114 void inverseComposition(
122 void drawSingleSample(
CPose2D &outPart )
const;
126 void drawManySamples(
size_t N, std::vector<mrpt::math::CVectorDouble> & outSamples )
const;
137 void bayesianFusion(
const CPosePDF &p1,
const CPosePDF &p2,
const double &minMahalanobisDistToDrop = 0 );
141 void inverse(CPosePDF &o)
const;
147 double evaluatePDF(
const CPose2D &x )
const;
150 double evaluateNormalizedPDF(
const CPose2D &x )
const;
156 void assureMinCovariance(
const double & minStdXY,
const double &minStdPhi );
163 this->inverseComposition(*
this,ref);
void getMean(CPose2D &mean_pose) const
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
CPosePDFGaussian(const CPosePDF &o)
Copy constructor, including transformations between other PDFs.
CPose2D BASE_IMPEXP operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
A gaussian distribution for 2D points.
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
A numeric matrix of compile-time fixed size.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
CPosePDFGaussian(const CPose3DPDF &o)
Copy constructor, including transformations between other PDFs.
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
mrpt::math::TPoint2D BASE_IMPEXP operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
A class used to store a 2D pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z].