Declares a class that represents a Probability Density function (PDF) of a 2D pose
This class implements that PDF as the following multi-modal Gaussian distribution:
Where the number of modes N is the size of CPosePDFSOG::m_modes
See mrpt::poses::CPosePDF for more details.
Definition at line 36 of file CPosePDFSOG.h.
#include <mrpt/poses/CPosePDFSOG.h>
Classes | |
struct | TGaussianMode |
The struct for each mode: More... | |
Public Types | |
typedef mrpt::aligned_containers< TGaussianMode >::vector_t | CListGaussianModes |
typedef CListGaussianModes::const_iterator | const_iterator |
typedef CListGaussianModes::iterator | iterator |
enum | { is_3D_val = 0 } |
enum | { is_PDF_val = 1 } |
typedef CPose2D | type_value |
The type of the state the PDF represents. More... | |
Public Member Functions | |
CPosePDFSOG (size_t nModes=1) | |
Default constructor. More... | |
size_t | size () const |
Return the number of Gaussian modes. More... | |
bool | empty () const |
Return whether there is any Gaussian mode. More... | |
void | clear () |
Clear the list of modes. More... | |
const TGaussianMode & | operator[] (size_t i) const |
Access to individual beacons. More... | |
TGaussianMode & | operator[] (size_t i) |
Access to individual beacons. More... | |
const TGaussianMode & | get (size_t i) const |
Access to individual beacons. More... | |
TGaussianMode & | get (size_t i) |
Access to individual beacons. More... | |
void | push_back (const TGaussianMode &m) |
Inserts a copy of the given mode into the SOG. More... | |
iterator | begin () |
iterator | end () |
const_iterator | begin () const |
const_iterator | end () const |
iterator | erase (iterator i) |
void | resize (const size_t N) |
Resize the number of SOG modes. More... | |
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 distribution. More... | |
void | getMean (CPose2D &mean_pose) const MRPT_OVERRIDE |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF) More... | |
void | getCovarianceAndMean (mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const MRPT_OVERRIDE |
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once. More... | |
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 the mean. More... | |
void | normalizeWeights () |
Normalize the weights in m_modes such as the maximum log-weight is 0. More... | |
void | copyFrom (const CPosePDF &o) MRPT_OVERRIDE |
Copy operator, translating if necesary (for example, between particles and gaussian representations) More... | |
void | saveToTextFile (const std::string &file) const MRPT_OVERRIDE |
Save the density to a text file, with the following format: There is one row per Gaussian "mode", and each row contains 10 elements: More... | |
void | changeCoordinatesReference (const CPose3D &newReferenceBase) MRPT_OVERRIDE |
this = p (+) this. More... | |
void | rotateAllCovariances (const double &ang) |
Rotate all the covariance matrixes by replacing them by ![]() ![]() | |
void | drawSingleSample (CPose2D &outPart) const MRPT_OVERRIDE |
Draws a single sample from the distribution. More... | |
void | drawManySamples (size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const MRPT_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. More... | |
void | inverse (CPosePDF &o) const MRPT_OVERRIDE |
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF. More... | |
void | operator+= (const mrpt::poses::CPose2D &Ap) |
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). More... | |
double | evaluatePDF (const mrpt::poses::CPose2D &x, bool sumOverAllPhis=false) const |
Evaluates the PDF at a given point. More... | |
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,1]. More... | |
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 matrix (each row contains values for a fixed y-coordinate value). More... | |
void | bayesianFusion (const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE |
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1 must be a mrpt::poses::CPosePDFSOG object and p2 a mrpt::poses::CPosePDFGaussian object) More... | |
template<class OPENGL_SETOFOBJECTSPTR > | |
void | getAs3DObject (OPENGL_SETOFOBJECTSPTR &out_obj) const |
Returns a 3D representation of this PDF (it doesn't clear the current contents of out_obj, but append new OpenGL objects to that list) More... | |
template<class OPENGL_SETOFOBJECTSPTR > | |
OPENGL_SETOFOBJECTSPTR | getAs3DObject () const |
Returns a 3D representation of this PDF. More... | |
virtual void | getMean (CPose2D &mean_point) const=0 |
Returns the mean, or mathematical expectation of the probability density distribution (PDF). More... | |
virtual void | getCovarianceAndMean (mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov, CPose2D &mean_point) const=0 |
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once. More... | |
void | getCovarianceDynAndMean (mrpt::math::CMatrixDouble &cov, CPose2D &mean_point) const |
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once. More... | |
CPose2D | getMeanVal () const |
Returns the mean, or mathematical expectation of the probability density distribution (PDF). More... | |
void | getCovariance (mrpt::math::CMatrixDouble &cov) const |
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) More... | |
void | getCovariance (mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov) const |
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) More... | |
mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > | getCovariance () const |
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) More... | |
virtual void | getInformationMatrix (mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &inf) const |
Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix) Unless reimplemented in derived classes, this method first reads the covariance, then invert it. More... | |
virtual void | drawSingleSample (CPose2D &outPart) const=0 |
Draws a single sample from the distribution. More... | |
double | getCovarianceEntropy () const |
Compute the entropy of the estimated covariance matrix. More... | |
Static Public Member Functions | |
static void | jacobiansPoseComposition (const CPose2D &x, const CPose2D &u, mrpt::math::CMatrixDouble33 &df_dx, mrpt::math::CMatrixDouble33 &df_du, const bool compute_df_dx=true, const bool compute_df_du=true) |
This static method computes the pose composition Jacobians, with these formulas: More... | |
static void | jacobiansPoseComposition (const CPosePDFGaussian &x, const CPosePDFGaussian &u, mrpt::math::CMatrixDouble33 &df_dx, mrpt::math::CMatrixDouble33 &df_du) |
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts. More... | |
static bool | is_3D () |
static bool | is_PDF () |
Static Public Attributes | |
static const size_t | state_length |
The length of the variable, for example, 3 for a 3D point, 6 for a 3D pose (x y z yaw pitch roll). More... | |
RTTI stuff <br> | |
static const mrpt::utils::TRuntimeClassId | classCPosePDF |
Protected Member Functions | |
void | assureSymmetry () |
Ensures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!) More... | |
CSerializable virtual methods | |
void | writeToStream (mrpt::utils::CStream &out, int *getVersion) const MRPT_OVERRIDE |
void | readFromStream (mrpt::utils::CStream &in, int version) MRPT_OVERRIDE |
Protected Attributes | |
CListGaussianModes | m_modes |
The list of SOG modes. More... | |
RTTI stuff <br> | |
typedef CPosePDFSOGPtr | SmartPtr |
static mrpt::utils::CLASSINIT | _init_CPosePDFSOG |
static mrpt::utils::TRuntimeClassId | classCPosePDFSOG |
static const mrpt::utils::TRuntimeClassId * | classinfo |
static const mrpt::utils::TRuntimeClassId * | _GetBaseClass () |
virtual const mrpt::utils::TRuntimeClassId * | GetRuntimeClass () const MRPT_OVERRIDE |
virtual mrpt::utils::CObject * | duplicate () const MRPT_OVERRIDE |
static mrpt::utils::CObject * | CreateObject () |
static CPosePDFSOGPtr | Create () |
typedef mrpt::aligned_containers<TGaussianMode>::vector_t mrpt::poses::CPosePDFSOG::CListGaussianModes |
Definition at line 63 of file CPosePDFSOG.h.
typedef CListGaussianModes::const_iterator mrpt::poses::CPosePDFSOG::const_iterator |
Definition at line 64 of file CPosePDFSOG.h.
typedef CListGaussianModes::iterator mrpt::poses::CPosePDFSOG::iterator |
Definition at line 65 of file CPosePDFSOG.h.
A typedef for the associated smart pointer
Definition at line 39 of file CPosePDFSOG.h.
|
inherited |
The type of the state the PDF represents.
Definition at line 32 of file CProbabilityDensityFunction.h.
|
inherited |
Enumerator | |
---|---|
is_3D_val |
Definition at line 91 of file CPosePDF.h.
|
inherited |
Enumerator | |
---|---|
is_PDF_val |
Definition at line 93 of file CPosePDF.h.
mrpt::poses::CPosePDFSOG::CPosePDFSOG | ( | size_t | nModes = 1 | ) |
Default constructor.
nModes | The initial size of CPosePDFSOG::m_modes |
|
staticprotected |
|
protected |
Ensures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)
|
virtual |
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1 must be a mrpt::poses::CPosePDFSOG object and p2 a mrpt::poses::CPosePDFGaussian object)
Implements mrpt::poses::CPosePDF.
|
inline |
Definition at line 110 of file CPosePDFSOG.h.
|
inline |
Definition at line 112 of file CPosePDFSOG.h.
|
virtual |
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.
Implements mrpt::utils::CProbabilityDensityFunction< CPose2D, 3 >.
void mrpt::poses::CPosePDFSOG::clear | ( | ) |
Clear the list of modes.
|
virtual |
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Implements mrpt::poses::CPosePDF.
|
static |
|
static |
|
virtual |
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.
Reimplemented from mrpt::utils::CProbabilityDensityFunction< CPose2D, 3 >.
void mrpt::poses::CPosePDFSOG::drawSingleSample | ( | CPose2D & | outPart | ) | const |
Draws a single sample from the distribution.
|
pure virtualinherited |
Draws a single sample from the distribution.
|
virtual |
|
inline |
Return whether there is any Gaussian mode.
Definition at line 78 of file CPosePDFSOG.h.
|
inline |
Definition at line 111 of file CPosePDFSOG.h.
|
inline |
Definition at line 113 of file CPosePDFSOG.h.
Definition at line 115 of file CPosePDFSOG.h.
double mrpt::poses::CPosePDFSOG::evaluateNormalizedPDF | ( | const mrpt::poses::CPose2D & | x | ) | const |
Evaluates the ratio PDF(x) / max_PDF(x*), that is, the normalized PDF in the range [0,1].
double mrpt::poses::CPosePDFSOG::evaluatePDF | ( | const mrpt::poses::CPose2D & | x, |
bool | sumOverAllPhis = false |
||
) | const |
Evaluates the PDF at a given point.
void mrpt::poses::CPosePDFSOG::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 matrix (each row contains values for a fixed y-coordinate value).
|
inline |
|
inline |
|
inlineinherited |
Returns a 3D representation of this PDF.
Definition at line 109 of file CPosePDF.h.
|
inlineinherited |
Returns a 3D representation of this PDF (it doesn't clear the current contents of out_obj, but append new OpenGL objects to that list)
Definition at line 100 of file CPosePDF.h.
|
inlineinherited |
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
Definition at line 85 of file CProbabilityDensityFunction.h.
|
inlineinherited |
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
Definition at line 67 of file CProbabilityDensityFunction.h.
|
inlineinherited |
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
Definition at line 76 of file CProbabilityDensityFunction.h.
void mrpt::poses::CPosePDFSOG::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.
|
pure virtualinherited |
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
|
inlineinherited |
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once.
Definition at line 47 of file CProbabilityDensityFunction.h.
|
inlineinherited |
Compute the entropy of the estimated covariance matrix.
Definition at line 136 of file CProbabilityDensityFunction.h.
|
inlinevirtualinherited |
Returns the information (inverse covariance) matrix (a STATE_LEN x STATE_LEN matrix) Unless reimplemented in derived classes, this method first reads the covariance, then invert it.
Definition at line 98 of file CProbabilityDensityFunction.h.
|
pure virtualinherited |
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
void mrpt::poses::CPosePDFSOG::getMean | ( | CPose2D & | mean_pose | ) | const |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
|
inlineinherited |
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
Definition at line 57 of file CProbabilityDensityFunction.h.
void mrpt::poses::CPosePDFSOG::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 the mean.
|
virtual |
Reimplemented from mrpt::poses::CPosePDF.
|
virtual |
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Implements mrpt::poses::CPosePDF.
|
inlinestaticinherited |
Definition at line 92 of file CPosePDF.h.
|
inlinestaticinherited |
Definition at line 94 of file CPosePDF.h.
|
staticinherited |
This static method computes the pose composition Jacobians, with these formulas:
Referenced by mrpt::math::jacobians::jacobs_2D_pose_comp().
|
staticinherited |
This is an overloaded member function, provided for convenience. It differs from the above function only in what argument(s) it accepts.
void mrpt::poses::CPosePDFSOG::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 distribution.
This method uses the approach described in the paper:
max_KLd | The maximum KL-divergence to consider the merge of two nodes (and then stops the process). |
void mrpt::poses::CPosePDFSOG::normalizeWeights | ( | ) |
Normalize the weights in m_modes such as the maximum log-weight is 0.
void mrpt::poses::CPosePDFSOG::operator+= | ( | const mrpt::poses::CPose2D & | Ap | ) |
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated).
|
inline |
|
inline |
|
inline |
Inserts a copy of the given mode into the SOG.
Definition at line 106 of file CPosePDFSOG.h.
|
protected |
void mrpt::poses::CPosePDFSOG::resize | ( | const size_t | N | ) |
Resize the number of SOG modes.
void mrpt::poses::CPosePDFSOG::rotateAllCovariances | ( | const double & | ang | ) |
Rotate all the covariance matrixes by replacing them by
|
virtual |
Save the density to a text file, with the following format: There is one row per Gaussian "mode", and each row contains 10 elements:
Implements mrpt::utils::CProbabilityDensityFunction< CPose2D, 3 >.
|
inline |
Return the number of Gaussian modes.
Definition at line 77 of file CPosePDFSOG.h.
|
protected |
|
staticprotected |
Definition at line 39 of file CPosePDFSOG.h.
|
staticinherited |
Definition at line 41 of file CPosePDF.h.
|
static |
Definition at line 39 of file CPosePDFSOG.h.
|
static |
Definition at line 39 of file CPosePDFSOG.h.
|
protected |
The list of SOG modes.
Definition at line 70 of file CPosePDFSOG.h.
|
staticinherited |
The length of the variable, for example, 3 for a 3D point, 6 for a 3D pose (x y z yaw pitch roll).
Definition at line 31 of file CProbabilityDensityFunction.h.
Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 03:17:04 UTC 2022 |