Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x,y,phi).
This class implements that PDF using a 3D grid.
Definition at line 30 of file CPosePDFGrid.h.
#include <mrpt/poses/CPosePDFGrid.h>
Public Types | |
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 | |
CPosePDFGrid (double xMin=-1.0f, double xMax=1.0f, double yMin=-1.0f, double yMax=1.0f, double resolutionXY=0.5f, double resolutionPhi=mrpt::utils::DEG2RAD(180), double phiMin=-M_PIf, double phiMax=M_PIf) | |
Constructor: Initializes a, uniform distribution over the whole given range. More... | |
virtual | ~CPosePDFGrid () |
Destructor. More... | |
void | copyFrom (const CPosePDF &o) MRPT_OVERRIDE |
Copy operator, translating if necesary (for example, between particles and gaussian representations) More... | |
void | normalize () |
Normalizes the PDF, such as all cells sum the unity. More... | |
void | uniformDistribution () |
Assigns the same value to all the cells in the grid, so the sum 1. 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 | saveToTextFile (const std::string &dataFile) const MRPT_OVERRIDE |
Save the contents of the 3D grid in one file, as a vertical concatenation of rectangular matrix for the different "PHI" discrete levels, and the size in X,Y,and PHI in another file named "<filename>_dims.txt". More... | |
void | changeCoordinatesReference (const CPose3D &newReferenceBase) MRPT_OVERRIDE |
this = p (+) this. More... | |
void | bayesianFusion (const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE |
Bayesian fusion of 2 densities (In the grid representation this becomes a pointwise multiplication) More... | |
void | inverse (CPosePDF &o) const MRPT_OVERRIDE |
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF. More... | |
void | drawSingleSample (CPose2D &outPart) const MRPT_OVERRIDE |
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) 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... | |
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... | |
size_t | x2idx (double x) const |
Returns "indexes" from coordinates: More... | |
size_t | y2idx (double y) const |
Returns "indexes" from coordinates: More... | |
size_t | phi2idx (double phi) const |
Returns "indexes" from coordinates: More... | |
double | idx2x (size_t x) const |
Returns coordinates from "indexes": More... | |
double | idx2y (size_t y) const |
Returns coordinates from "indexes": More... | |
double | idx2phi (size_t phi) const |
Returns coordinates from "indexes": More... | |
void | setSize (double xMin, double xMax, double yMin, double yMax, double resolutionXY, double resolutionPhi, double phiMin=-M_PIf, double phiMax=M_PIf) |
Changes the limits and size of the grid, erasing previous contents: More... | |
const double * | getByPos (double x, double y, double phi) const |
Reads the contents of a cell. More... | |
double * | getByPos (double x, double y, double phi) |
Reads the contents of a cell. More... | |
const double * | getByIndex (size_t x, size_t y, size_t phi) const |
Reads the contents of a cell. More... | |
double * | getByIndex (size_t x, size_t y, size_t phi) |
Reads the contents of a cell. More... | |
void | getAsMatrix (const double &phi, MATRIXLIKE &outMat) |
Returns the whole grid as a matrix, for a given constant "phi" and where each row contains values for a fixed "y". More... | |
double | getXMin () const |
Get info about the grid: More... | |
double | getXMax () const |
double | getYMin () const |
double | getYMax () const |
double | getPhiMin () const |
double | getPhiMax () const |
double | getResolutionXY () const |
double | getResolutionPhi () const |
size_t | getSizeX () const |
size_t | getSizeY () const |
size_t | getSizePhi () const |
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 | |
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 | |
double | m_xMin |
The limits and resolution of the grid: More... | |
double | m_xMax |
double | m_yMin |
double | m_yMax |
double | m_phiMin |
double | m_phiMax |
double | m_resolutionXY |
double | m_resolutionPhi |
size_t | m_sizeX |
The size of "m_data" is m_sizeX * m_sizeY * m_sizePhi. More... | |
size_t | m_sizeY |
size_t | m_sizePhi |
size_t | m_sizeXY |
int | m_idxLeftX |
The indexes of the "left" borders: More... | |
int | m_idxLeftY |
int | m_idxLeftPhi |
std::vector< double > | m_data |
The data: More... | |
RTTI stuff <br> | |
typedef CPosePDFGridPtr | SmartPtr |
static mrpt::utils::CLASSINIT | _init_CPosePDFGrid |
static mrpt::utils::TRuntimeClassId | classCPosePDFGrid |
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 CPosePDFGridPtr | Create () |
A typedef for the associated smart pointer
Definition at line 33 of file CPosePDFGrid.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::CPosePDFGrid::CPosePDFGrid | ( | double | xMin = -1.0f , |
double | xMax = 1.0f , |
||
double | yMin = -1.0f , |
||
double | yMax = 1.0f , |
||
double | resolutionXY = 0.5f , |
||
double | resolutionPhi = mrpt::utils::DEG2RAD(180) , |
||
double | phiMin = -M_PIf , |
||
double | phiMax = M_PIf |
||
) |
Constructor: Initializes a, uniform distribution over the whole given range.
|
virtual |
Destructor.
|
staticprotected |
|
virtual |
Bayesian fusion of 2 densities (In the grid representation this becomes a pointwise multiplication)
Implements mrpt::poses::CPosePDF.
|
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 >.
|
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::CPosePDFGrid::drawSingleSample | ( | CPose2D & | outPart | ) | const |
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
|
pure virtualinherited |
Draws a single sample from the distribution.
|
virtual |
|
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 whole grid as a matrix, for a given constant "phi" and where each row contains values for a fixed "y".
Definition at line 200 of file CPose2DGridTemplate.h.
|
inlineinherited |
Reads the contents of a cell.
Definition at line 191 of file CPose2DGridTemplate.h.
|
inlineinherited |
Reads the contents of a cell.
Definition at line 183 of file CPose2DGridTemplate.h.
|
inlineinherited |
Reads the contents of a cell.
Definition at line 176 of file CPose2DGridTemplate.h.
|
inlineinherited |
Reads the contents of a cell.
Definition at line 169 of file CPose2DGridTemplate.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::CPosePDFGrid::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::CPosePDFGrid::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.
|
inlineinherited |
Definition at line 219 of file CPose2DGridTemplate.h.
|
inlineinherited |
Definition at line 218 of file CPose2DGridTemplate.h.
|
inlineinherited |
Definition at line 221 of file CPose2DGridTemplate.h.
|
inlineinherited |
Definition at line 220 of file CPose2DGridTemplate.h.
|
virtual |
Reimplemented from mrpt::poses::CPosePDF.
|
inlineinherited |
Definition at line 224 of file CPose2DGridTemplate.h.
|
inlineinherited |
Definition at line 222 of file CPose2DGridTemplate.h.
|
inlineinherited |
Definition at line 223 of file CPose2DGridTemplate.h.
|
inlineinherited |
Definition at line 215 of file CPose2DGridTemplate.h.
|
inlineinherited |
Get info about the grid:
Definition at line 214 of file CPose2DGridTemplate.h.
|
inlineinherited |
Definition at line 217 of file CPose2DGridTemplate.h.
|
inlineinherited |
Definition at line 216 of file CPose2DGridTemplate.h.
|
inlineinherited |
Returns coordinates from "indexes":
Definition at line 92 of file CPose2DGridTemplate.h.
|
inlineinherited |
Returns coordinates from "indexes":
Definition at line 76 of file CPose2DGridTemplate.h.
|
inlineinherited |
Returns coordinates from "indexes":
Definition at line 84 of file CPose2DGridTemplate.h.
|
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::CPosePDFGrid::normalize | ( | ) |
Normalizes the PDF, such as all cells sum the unity.
|
inlineinherited |
Returns "indexes" from coordinates:
Definition at line 67 of file CPose2DGridTemplate.h.
|
protected |
|
virtual |
Save the contents of the 3D grid in one file, as a vertical concatenation of rectangular matrix for the different "PHI" discrete levels, and the size in X,Y,and PHI in another file named "<filename>_dims.txt".
Implements mrpt::utils::CProbabilityDensityFunction< CPose2D, 3 >.
|
inlineinherited |
Changes the limits and size of the grid, erasing previous contents:
Definition at line 126 of file CPose2DGridTemplate.h.
void mrpt::poses::CPosePDFGrid::uniformDistribution | ( | ) |
Assigns the same value to all the cells in the grid, so the sum 1.
|
protected |
|
inlineinherited |
Returns "indexes" from coordinates:
Definition at line 49 of file CPose2DGridTemplate.h.
|
inlineinherited |
Returns "indexes" from coordinates:
Definition at line 58 of file CPose2DGridTemplate.h.
|
staticprotected |
Definition at line 33 of file CPosePDFGrid.h.
|
staticinherited |
Definition at line 41 of file CPosePDF.h.
|
static |
Definition at line 33 of file CPosePDFGrid.h.
|
static |
Definition at line 33 of file CPosePDFGrid.h.
|
protectedinherited |
The data:
Definition at line 44 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 40 of file CPose2DGridTemplate.h.
|
protectedinherited |
The indexes of the "left" borders:
Definition at line 40 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 40 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 31 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 31 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 32 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 32 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 36 of file CPose2DGridTemplate.h.
|
protectedinherited |
The size of "m_data" is m_sizeX * m_sizeY * m_sizePhi.
Definition at line 36 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 36 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 36 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 29 of file CPose2DGridTemplate.h.
|
protectedinherited |
The limits and resolution of the grid:
Definition at line 29 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 30 of file CPose2DGridTemplate.h.
|
protectedinherited |
Definition at line 30 of file CPose2DGridTemplate.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 Tue Dec 27 00:53:09 UTC 2022 |