Declares a class that represents a Probability Density function (PDF) of a 3D pose
This class implements that PDF using a mono-modal Gaussian distribution. See mrpt::poses::CPose3DPDF for more details.
Uncertainty of pose composition operations (
For further details on implemented methods and the theory behind them, see this report.
Definition at line 37 of file CPose3DPDFGaussian.h.
#include <mrpt/poses/CPose3DPDFGaussian.h>
Public Types | |
enum | { is_3D_val = 1 } |
enum | { is_PDF_val = 1 } |
typedef CPose3D | type_value |
The type of the state the PDF represents. | |
Public Member Functions | |
CPose3DPDFGaussian () | |
Default constructor. | |
CPose3DPDFGaussian (const CPose3D &init_Mean) | |
Constructor. | |
CPose3DPDFGaussian (TConstructorFlags_Poses constructor_dummy_param) | |
Uninitialized constructor: leave all fields uninitialized - Call with UNINITIALIZED_POSE as argument. | |
CPose3DPDFGaussian (const CPose3D &init_Mean, const mrpt::math::CMatrixDouble66 &init_Cov) | |
Constructor | |
CPose3DPDFGaussian (const CPosePDFGaussian &o) | |
Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables z,pitch, and roll). | |
CPose3DPDFGaussian (const CPose3DQuatPDFGaussian &o) | |
Constructor from a 6D pose PDF described as a Quaternion. | |
const CPose3D & | getPoseMean () const |
CPose3D & | getPoseMean () |
void | getMean (CPose3D &mean_pose) const MRPT_OVERRIDE |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). | |
void | getCovarianceAndMean (mrpt::math::CMatrixDouble66 &cov, CPose3D &mean_point) const MRPT_OVERRIDE |
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once. | |
void | asString (std::string &s) const |
std::string | asString () const |
void | copyFrom (const CPose3DPDF &o) MRPT_OVERRIDE |
Copy operator, translating if necesary (for example, between particles and gaussian representations) | |
void | copyFrom (const CPosePDF &o) |
Copy operator, translating if necesary (for example, between particles and gaussian representations) | |
void | copyFrom (const CPose3DQuatPDFGaussian &o) |
Copy from a 6D pose PDF described as a Quaternion. | |
void | saveToTextFile (const std::string &file) const MRPT_OVERRIDE |
Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in next 3 lines. | |
void | changeCoordinatesReference (const CPose3D &newReferenceBase) MRPT_OVERRIDE |
this = p (+) this. | |
void | drawSingleSample (CPose3D &outPart) const MRPT_OVERRIDE |
Draws a single sample from the distribution. | |
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 1x6 vectors, where each row contains a (x,y,phi) datum. | |
void | bayesianFusion (const CPose3DPDF &p1, const CPose3DPDF &p2) MRPT_OVERRIDE |
Bayesian fusion of two points gauss. | |
void | inverse (CPose3DPDF &o) const MRPT_OVERRIDE |
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF. | |
CPose3DPDFGaussian | operator- () const |
Unary - operator, returns the PDF of the inverse pose. | |
void | operator+= (const CPose3D &Ap) |
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). | |
void | operator+= (const CPose3DPDFGaussian &Ap) |
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). | |
void | operator-= (const CPose3DPDFGaussian &Ap) |
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated). | |
double | evaluatePDF (const CPose3D &x) const |
Evaluates the PDF at a given point. | |
double | evaluateNormalizedPDF (const CPose3D &x) const |
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1]. | |
double | mahalanobisDistanceTo (const CPose3DPDFGaussian &theOther) |
Computes the Mahalanobis distance between the centers of two Gaussians. | |
void | getCovSubmatrix2D (mrpt::math::CMatrixDouble &out_cov) const |
Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only. | |
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) | |
template<class OPENGL_SETOFOBJECTSPTR > | |
OPENGL_SETOFOBJECTSPTR | getAs3DObject () const |
Returns a 3D representation of this PDF. | |
virtual void | getMean (CPose3D &mean_point) const=0 |
Returns the mean, or mathematical expectation of the probability density distribution (PDF). | |
virtual void | getCovarianceAndMean (mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov, CPose3D &mean_point) const=0 |
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once. | |
void | getCovarianceDynAndMean (mrpt::math::CMatrixDouble &cov, CPose3D &mean_point) const |
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean, both at once. | |
CPose3D | getMeanVal () const |
Returns the mean, or mathematical expectation of the probability density distribution (PDF). | |
void | getCovariance (mrpt::math::CMatrixDouble &cov) const |
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) | |
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) | |
mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > | getCovariance () const |
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) | |
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. | |
virtual void | drawSingleSample (CPose3D &outPart) const=0 |
Draws a single sample from the distribution. | |
double | getCovarianceEntropy () const |
Compute the entropy of the estimated covariance matrix. | |
Static Public Member Functions | |
static CPose3DPDF * | createFrom2D (const CPosePDF &o) |
This is a static transformation method from 2D poses to 3D PDFs, preserving the representation type (particles->particles, Gaussians->Gaussians,etc) It returns a new object of any of the derived classes of CPose3DPDF. | |
static void | jacobiansPoseComposition (const CPose3D &x, const CPose3D &u, mrpt::math::CMatrixDouble66 &df_dx, mrpt::math::CMatrixDouble66 &df_du) |
This static method computes the pose composition Jacobians. | |
static bool | is_3D () |
static bool | is_PDF () |
Public Attributes | |
CPose3D | mean |
The mean value. | |
mrpt::math::CMatrixDouble66 | cov |
The 6x6 covariance matrix. | |
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). | |
RTTI stuff <br> | |
static const mrpt::utils::TRuntimeClassId | classCPose3DPDF |
Protected Member Functions | |
void | assureSymmetry () |
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!) | |
CSerializable virtual methods | |
void | writeToStream (mrpt::utils::CStream &out, int *getVersion) const MRPT_OVERRIDE |
void | readFromStream (mrpt::utils::CStream &in, int version) MRPT_OVERRIDE |
RTTI stuff <br> | |
typedef CPose3DPDFGaussianPtr | SmartPtr |
static mrpt::utils::CLASSINIT | _init_CPose3DPDFGaussian |
static mrpt::utils::TRuntimeClassId | classCPose3DPDFGaussian |
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 CPose3DPDFGaussianPtr | Create () |
A typedef for the associated smart pointer
Definition at line 40 of file CPose3DPDFGaussian.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 CPose3DPDF.h.
|
inherited |
Enumerator | |
---|---|
is_PDF_val |
Definition at line 93 of file CPose3DPDF.h.
mrpt::poses::CPose3DPDFGaussian::CPose3DPDFGaussian | ( | ) |
Default constructor.
|
explicit |
Constructor.
mrpt::poses::CPose3DPDFGaussian::CPose3DPDFGaussian | ( | TConstructorFlags_Poses | constructor_dummy_param | ) |
Uninitialized constructor: leave all fields uninitialized - Call with UNINITIALIZED_POSE as argument.
mrpt::poses::CPose3DPDFGaussian::CPose3DPDFGaussian | ( | const CPose3D & | init_Mean, |
const mrpt::math::CMatrixDouble66 & | init_Cov | ||
) |
Constructor
|
explicit |
Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables z,pitch, and roll).
|
explicit |
Constructor from a 6D pose PDF described as a Quaternion.
|
staticprotected |
|
inline |
Definition at line 98 of file CPose3DPDFGaussian.h.
References asString().
Referenced by asString().
void mrpt::poses::CPose3DPDFGaussian::asString | ( | std::string & | s | ) | const |
|
protected |
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)
|
virtual |
Bayesian fusion of two points gauss.
distributions, then save the result in this object. The process is as follows:
S = (S1-1 + S2-1)-1; x = S * ( S1-1*x1 + S2-1*x2 );
Implements mrpt::poses::CPose3DPDF.
|
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< CPose3D, 6 >.
|
virtual |
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Implements mrpt::poses::CPose3DPDF.
void mrpt::poses::CPose3DPDFGaussian::copyFrom | ( | const CPose3DQuatPDFGaussian & | o | ) |
Copy from a 6D pose PDF described as a Quaternion.
void mrpt::poses::CPose3DPDFGaussian::copyFrom | ( | const CPosePDF & | o | ) |
Copy operator, translating if necesary (for example, between particles and gaussian representations)
|
static |
|
staticinherited |
This is a static transformation method from 2D poses to 3D PDFs, preserving the representation type (particles->particles, Gaussians->Gaussians,etc) It returns a new object of any of the derived classes of CPose3DPDF.
This object must be deleted by the user when not required anymore.
|
static |
|
virtual |
Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum.
Reimplemented from mrpt::utils::CProbabilityDensityFunction< CPose3D, 6 >.
void mrpt::poses::CPose3DPDFGaussian::drawSingleSample | ( | CPose3D & | outPart | ) | const |
Draws a single sample from the distribution.
|
pure virtualinherited |
Draws a single sample from the distribution.
|
virtual |
double mrpt::poses::CPose3DPDFGaussian::evaluateNormalizedPDF | ( | const CPose3D & | x | ) | const |
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1].
double mrpt::poses::CPose3DPDFGaussian::evaluatePDF | ( | const CPose3D & | x | ) | const |
Evaluates the PDF at a given point.
|
inlineinherited |
Returns a 3D representation of this PDF.
Definition at line 109 of file CPose3DPDF.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 CPose3DPDF.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.
|
inline |
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once.
Definition at line 92 of file CPose3DPDFGaussian.h.
References mean().
|
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.
void mrpt::poses::CPose3DPDFGaussian::getCovSubmatrix2D | ( | mrpt::math::CMatrixDouble & | out_cov | ) | const |
Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only.
|
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).
|
inline |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
Definition at line 85 of file CPose3DPDFGaussian.h.
References mean().
Referenced by mrpt::slam::PF_implementation< PARTICLE_TYPE, MYSELF >::PF_SLAM_implementation_pfStandardProposal().
|
inlineinherited |
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
Definition at line 57 of file CProbabilityDensityFunction.h.
|
inline |
Definition at line 80 of file CPose3DPDFGaussian.h.
References mean().
|
inline |
Definition at line 79 of file CPose3DPDFGaussian.h.
References mean().
|
virtual |
Reimplemented from mrpt::poses::CPose3DPDF.
|
virtual |
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Implements mrpt::poses::CPose3DPDF.
|
inlinestaticinherited |
Definition at line 92 of file CPose3DPDF.h.
|
inlinestaticinherited |
Definition at line 94 of file CPose3DPDF.h.
|
staticinherited |
This static method computes the pose composition Jacobians.
See this techical report: http:///www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty
Direct equations (for the covariances) in yaw-pitch-roll are too complex. Make a way around them and consider instead this path:
Referenced by mrpt::math::jacobians::jacobs_6D_pose_comp().
double mrpt::poses::CPose3DPDFGaussian::mahalanobisDistanceTo | ( | const CPose3DPDFGaussian & | theOther | ) |
Computes the Mahalanobis distance between the centers of two Gaussians.
The variables with a variance exactly equal to 0 are not taken into account in the process, but "infinity" is returned if the corresponding elements are not exactly equal.
void mrpt::poses::CPose3DPDFGaussian::operator+= | ( | const CPose3D & | Ap | ) |
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated).
void mrpt::poses::CPose3DPDFGaussian::operator+= | ( | const CPose3DPDFGaussian & | Ap | ) |
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated).
|
inline |
Unary - operator, returns the PDF of the inverse pose.
Definition at line 146 of file CPose3DPDFGaussian.h.
References mrpt::poses::UNINITIALIZED_POSE.
void mrpt::poses::CPose3DPDFGaussian::operator-= | ( | const CPose3DPDFGaussian & | Ap | ) |
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated).
|
protected |
|
virtual |
Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in next 3 lines.
Implements mrpt::utils::CProbabilityDensityFunction< CPose3D, 6 >.
|
protected |
|
staticprotected |
Definition at line 40 of file CPose3DPDFGaussian.h.
|
staticinherited |
Definition at line 42 of file CPose3DPDF.h.
|
static |
Definition at line 40 of file CPose3DPDFGaussian.h.
|
static |
Definition at line 40 of file CPose3DPDFGaussian.h.
mrpt::math::CMatrixDouble66 mrpt::poses::CPose3DPDFGaussian::cov |
The 6x6 covariance matrix.
Definition at line 77 of file CPose3DPDFGaussian.h.
Referenced by mrpt::graphs::detail::graph_ops< graph_t >::auxMaha2Dist().
CPose3D mrpt::poses::CPose3DPDFGaussian::mean |
The mean value.
Definition at line 73 of file CPose3DPDFGaussian.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.6 for MRPT 1.4.0 SVN: at Wed Feb 15 01:46:32 UTC 2023 |