59 o <<
"Mean: " <<
mode.mean << std::endl
60 <<
"Covariance: " << std::endl
61 <<
mode.cov << std::endl
62 <<
"Log-weight: " <<
mode.log_w << std::endl;
127 void resize(
const size_t N);
138 void mergeModes(
double max_KLd = 0.5,
bool verbose =
false);
191 std::vector<mrpt::math::CVectorDouble>& outSamples)
const override;
210 const double& x_min,
const double& x_max,
const double& y_min,
211 const double& y_max,
const double& resolutionXY,
const double& phi,
219 const double& minMahalanobisDistToDrop = 0)
override;
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
void clear()
Clear the list of modes.
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void mergeModes(double max_KLd=0.5, bool verbose=false)
Merge very close modes so the overall number of modes is reduced while preserving the total distribut...
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
The struct for each mode:
void saveToTextFile(const std::string &file) const override
Save the density to a text file, with the following format: There is one row per Gaussian "mode"...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
double evaluatePDF(const mrpt::poses::CPose2D &x, bool sumOverAllPhis=false) const
Evaluates the PDF at a given point.
const TGaussianMode & operator[](size_t i) const
Access to individual beacons.
void getMean(CPose2D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF) ...
mrpt::aligned_containers< TGaussianMode >::vector_t CListGaussianModes
const Scalar * const_iterator
void push_back(const TGaussianMode &m)
Inserts a copy of the given mode into the SOG.
CPosePDFSOG(size_t nModes=1)
Default constructor.
void rotateAllCovariances(const double &ang)
Rotate all the covariance matrixes by replacing them by , where .
friend std::ostream & operator<<(std::ostream &o, const TGaussianMode &mode)
MRPT_MAKE_ALIGNED_OPERATOR_NEW
TGaussianMode & operator[](size_t i)
Access to individual beacons.
const_iterator end() const
void evaluatePDFInArea(const double &x_min, const double &x_max, const double &y_min, const double &y_max, const double &resolutionXY, const double &phi, mrpt::math::CMatrixD &outMatrix, bool sumOverAllPhis=false)
Evaluates the PDF within a rectangular grid (and a fixed orientation) and saves the result in a matri...
const CListGaussianModes & getSOGModes() const
mrpt::math::CMatrixDouble33 cov
bool empty() const
Return whether there is any Gaussian mode.
GLsizei const GLchar ** string
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
double evaluateNormalizedPDF(const mrpt::poses::CPose2D &x) const
Evaluates the ratio PDF(x) / max_PDF(x*), that is, the normalized PDF in the range [0...
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const override
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once...
void assureSymmetry()
Ensures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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...
void getMostLikelyCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const
For the most likely Gaussian mode in the SOG, returns the pose covariance matrix (3x3 cov matrix) and...
iterator erase(iterator i)
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 drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const override
Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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...
size_t size() const
Return the number of Gaussian modes.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) override
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1...
CListGaussianModes::iterator iterator
void operator+=(const mrpt::poses::CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
double log_w
The log-weight.
const_iterator begin() const
std::vector< TYPE1, Eigen::aligned_allocator< TYPE1 > > vector_t
CListGaussianModes::const_iterator const_iterator
void resize(const size_t N)
Resize the number of SOG modes.
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
CListGaussianModes m_modes
The list of SOG modes.