#include <mrpt/poses/CPose3DPDFGaussian.h>
Public Member Functions | |
CPose3DPDFGaussian () | |
Default constructor. | |
CPose3DPDFGaussian (const CPose3D &init_Mean) | |
Constructor. | |
CPose3DPDFGaussian (const CPose3D &init_Mean, const CMatrixD &init_Cov) | |
Constructor. | |
CPose3DPDFGaussian (const CPosePDFGaussian &o) | |
Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables z,pitch, and roll). | |
CPose3D | getEstimatedPose () const |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). | |
CMatrixD | getEstimatedCovariance () const |
Returns an estimate of the pose covariance matrix (6x6 cov.matrix for x,y,z,yaw,pitch,and roll variables). | |
void | copyFrom (const CPose3DPDF &o) |
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 | saveToTextFile (const std::string &file) const |
Save PDF's particles 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) |
This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which "to project" the current pdf. | |
void | rotateCov (const double &ang) |
Rotate the covariance matrix by replacing it by ![]() ![]() | |
void | drawSingleSample (CPose3D &outPart) const |
Draws a single sample from the distribution. | |
void | drawManySamples (size_t N, std::vector< vector_double > &outSamples) const |
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 (CPose3DPDF &p1, CPose3DPDF &p2) |
Bayesian fusion of two points gauss. | |
void | inverse (CPose3DPDF &o) const |
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF. | |
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) (see formulas on the top of this page). | |
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 (CMatrixDouble &out_cov) const |
Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only. | |
Static Public Member Functions | |
static void | jacobiansPoseComposition (const CPose3DPDFGaussian &x, const CPose3DPDFGaussian &u, CMatrixDouble &df_dx, CMatrixDouble &df_du) |
This static method computes the pose composition Jacobians (see formulas on the top of this page). | |
Public Attributes | |
CPose3D | mean |
The mean value. | |
CMatrixD | cov |
The 6x6 covariance matrix. | |
Protected Member Functions | |
void | assureSymmetry () |
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!). |
This class implements that PDF using a mono-modal Gaussian distribution. See mrpt::poses::CPose3DPDF for more details.
The formulas used for pose/uncertainty composition in the "CPose3DPDFGaussian::operator+=" and "CPose3DPDFGaussian::jacobiansPoseComposition":
We compose the pose "x" by adding "u" to it, obtaining "y":
The mean of "y" is:
And its covariance:
With the Jacobians being computed by CPose3DPDFGaussian::jacobiansPoseComposition (check the source code for the exact formulas).
Definition at line 76 of file CPose3DPDFGaussian.h.
mrpt::poses::CPose3DPDFGaussian::CPose3DPDFGaussian | ( | ) |
Default constructor.
mrpt::poses::CPose3DPDFGaussian::CPose3DPDFGaussian | ( | const CPose3D & | init_Mean | ) |
Constructor.
mrpt::poses::CPose3DPDFGaussian::CPose3DPDFGaussian | ( | const CPose3D & | init_Mean, | |
const CMatrixD & | init_Cov | |||
) |
Constructor.
mrpt::poses::CPose3DPDFGaussian::CPose3DPDFGaussian | ( | const CPosePDFGaussian & | o | ) |
Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables z,pitch, and roll).
void mrpt::poses::CPose3DPDFGaussian::assureSymmetry | ( | ) | [protected] |
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!).
void mrpt::poses::CPose3DPDFGaussian::bayesianFusion | ( | CPose3DPDF & | p1, | |
CPose3DPDF & | p2 | |||
) | [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.
void mrpt::poses::CPose3DPDFGaussian::changeCoordinatesReference | ( | const CPose3D & | newReferenceBase | ) | [virtual] |
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::poses::CPose3DPDF.
void mrpt::poses::CPose3DPDFGaussian::copyFrom | ( | const CPosePDF & | o | ) |
Copy operator, translating if necesary (for example, between particles and gaussian representations).
void mrpt::poses::CPose3DPDFGaussian::copyFrom | ( | const CPose3DPDF & | o | ) | [virtual] |
Copy operator, translating if necesary (for example, between particles and gaussian representations).
Implements mrpt::poses::CPose3DPDF.
void mrpt::poses::CPose3DPDFGaussian::drawManySamples | ( | size_t | N, | |
std::vector< vector_double > & | outSamples | |||
) | const [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.
Implements mrpt::poses::CPose3DPDF.
void mrpt::poses::CPose3DPDFGaussian::drawSingleSample | ( | CPose3D & | outPart | ) | const [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.
void mrpt::poses::CPose3DPDFGaussian::getCovSubmatrix2D | ( | CMatrixDouble & | out_cov | ) | const |
Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only.
CMatrixD mrpt::poses::CPose3DPDFGaussian::getEstimatedCovariance | ( | ) | const [virtual] |
Returns an estimate of the pose covariance matrix (6x6 cov.matrix for x,y,z,yaw,pitch,and roll variables).
Implements mrpt::poses::CPose3DPDF.
CPose3D mrpt::poses::CPose3DPDFGaussian::getEstimatedPose | ( | ) | const [virtual] |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
Implements mrpt::poses::CPose3DPDF.
void mrpt::poses::CPose3DPDFGaussian::inverse | ( | CPose3DPDF & | o | ) | const [virtual] |
static void mrpt::poses::CPose3DPDFGaussian::jacobiansPoseComposition | ( | const CPose3DPDFGaussian & | x, | |
const CPose3DPDFGaussian & | u, | |||
CMatrixDouble & | df_dx, | |||
CMatrixDouble & | df_du | |||
) | [static] |
This static method computes the pose composition Jacobians (see formulas on the top of this page).
double mrpt::poses::CPose3DPDFGaussian::mahalanobisDistanceTo | ( | const CPose3DPDFGaussian & | theOther | ) |
Computes the Mahalanobis distance between the centers of two Gaussians.
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) (see formulas on the top of this page).
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::rotateCov | ( | const double & | ang | ) |
Rotate the covariance matrix by replacing it by , where
.
void mrpt::poses::CPose3DPDFGaussian::saveToTextFile | ( | const std::string & | file | ) | const [virtual] |
Save PDF's particles to a text file, containing the 3D pose in the first line, then the covariance matrix in next 3 lines.
Implements mrpt::poses::CPose3DPDF.
Page generated by Doxygen 1.5.7.1 for MRPT 0.6.5 SVN: at Mon Feb 23 13:35:09 EST 2009 |