#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). | |
void | getMean (CPose3D &mean_pose) const |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles. | |
void | getCovarianceAndMean (CMatrixDouble66 &cov, CPose3D &mean_point) const |
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once. | |
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, CMatrixDouble66 &df_dx, CMatrixDouble66 &df_du) |
This static method computes the pose composition Jacobians, with these formulas:. | |
Public Attributes | |
CPose3D | mean |
The mean value. | |
CMatrixDouble66 | 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 (see that method documentation 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::utils::CProbabilityDensityFunction< TDATA, STATE_LEN >.
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.
Reimplemented from mrpt::utils::CProbabilityDensityFunction< TDATA, STATE_LEN >.
void mrpt::poses::CPose3DPDFGaussian::drawSingleSample | ( | CPose3D & | outPart | ) | const |
Draws a single sample from the distribution.
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::getCovarianceAndMean | ( | CMatrixDouble66 & | cov, | |
CPose3D & | mean_point | |||
) | const |
Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once.
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.
void mrpt::poses::CPose3DPDFGaussian::getMean | ( | CPose3D & | mean_pose | ) | const |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
void mrpt::poses::CPose3DPDFGaussian::inverse | ( | CPose3DPDF & | o | ) | const [virtual] |
static void mrpt::poses::CPose3DPDFGaussian::jacobiansPoseComposition | ( | const CPose3DPDFGaussian & | x, | |
const CPose3DPDFGaussian & | u, | |||
CMatrixDouble66 & | df_dx, | |||
CMatrixDouble66 & | df_du | |||
) | [static] |
This static method computes the pose composition Jacobians, with these formulas:.
df_dx = [ 1, 0, 0, -sin(yaw)*cos(p)*xu+(-sin(yaw)*sin(p)*sin(r)-cos(yaw)*cos(r))*yu+(-sin(yaw)*sin(p)*cos(r)+cos(yaw)*sin(r))*zu, -cos(yaw)*sin(p)*xu+cos(yaw)*cos(p)*sin(r)*yu+cos(yaw)*cos(p)*cos(r)*zu, (cos(yaw)*sin(p)*cos(r)+sin(yaw)*sin(r))*yu+(-cos(yaw)*sin(p)*sin(r)+sin(yaw)*cos(r))*zu] [ 0, 1, 0, cos(yaw)*cos(p)*xu+(cos(yaw)*sin(p)*sin(r)-sin(yaw)*cos(r))*yu+(cos(yaw)*sin(p)*cos(r)+sin(yaw)*sin(r))*zu, -sin(yaw)*sin(p)*xu+sin(yaw)*cos(p)*sin(r)*yu+sin(yaw)*cos(p)*cos(r)*zu, (sin(yaw)*sin(p)*cos(r)-cos(yaw)*sin(r))*yu+(-sin(yaw)*sin(p)*sin(r)-cos(yaw)*cos(r))*zu] [ 0, 0, 1, 0, -cos(p)*xu-sin(p)*sin(r)*yu-sin(p)*cos(r)*zu, cos(p)*cos(r)*yu-cos(p)*sin(r)*zu] [ 0, 0, 0, 1, 0, 0] [ 0, 0, 0, 0, 1, 0] [ 0, 0, 0, 0, 0, 1] df_du = [ cos(yaw)*cos(p), cos(yaw)*sin(p)*sin(r)-sin(yaw)*cos(r), cos(yaw)*sin(p)*cos(r)+sin(yaw)*sin(r), 0, 0, 0] [ sin(yaw)*cos(p), sin(yaw)*sin(p)*sin(r)+cos(yaw)*cos(r), sin(yaw)*sin(p)*cos(r)-cos(yaw)*sin(r), 0, 0, 0] [ -sin(p), cos(p)*sin(r), cos(p)*cos(r), 0, 0, 0] [ 0, 0, 0, 1, 0, 0] [ 0, 0, 0, 0, 1, 0] [ 0, 0, 0, 0, 0, 1]
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 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::utils::CProbabilityDensityFunction< TDATA, STATE_LEN >.
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:02:22 EDT 2009 |