Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.
This class is also the base for the implementation of Monte-Carlo Localization (MCL), in mrpt::slam::CMonteCarloLocalization2D.
See the application "app/pf-localization" for an example of usage.
Definition at line 53 of file CPosePDFParticles.h.
#include <mrpt/poses/CPosePDFParticles.h>
Public Member Functions | |
IMPLEMENT_PARTICLE_FILTER_CAPABLE (CPose2D) | |
void | clear () |
Free all the memory associated to m_particles, and set the number of parts = 0. | |
CPosePDFParticles (size_t M=1) | |
Constructor. | |
CPosePDFParticles (const CPosePDFParticles &obj) | |
Copy constructor: | |
virtual | ~CPosePDFParticles () |
Destructor. | |
void | copyFrom (const CPosePDF &o) |
Copy operator, translating if necesary (for example, between m_particles and gaussian representations) | |
void | resetDeterministic (const CPose2D &location, size_t particlesCount=0) |
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose. | |
void | resetUniform (const double &x_min, const double &x_max, const double &y_min, const double &y_max, const double &phi_min=-M_PI, const double &phi_max=M_PI, const int &particlesCount=-1) |
Reset the PDF to an uniformly distributed one, inside of the defined cube. | |
void | getMean (CPose2D &mean_pose) const |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). | |
void | getCovarianceAndMean (CMatrixDouble33 &cov, CPose2D &mean_point) const |
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once. | |
CPose2D | getParticlePose (size_t i) const |
Returns the pose of the i'th particle. | |
void | saveToTextFile (const std::string &file) const |
Save PDF's m_particles to a text file. | |
size_t | size () const |
Get the m_particles count (equivalent to "particlesCount") | |
void | performSubstitution (std::vector< int > &indx) |
Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly. | |
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 | drawSingleSample (CPose2D &outPart) const |
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) | |
void | operator+= (const CPose2D &Ap) |
Appends (pose-composition) a given pose "p" to each particle. | |
void | append (CPosePDFParticles &o) |
Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights. | |
void | inverse (CPosePDF &o) const |
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF. | |
CPose2D | getMostLikelyParticle () const |
Returns the particle with the highest weight. | |
void | bayesianFusion (const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) |
Bayesian fusion. | |
double | evaluatePDF_parzen (const double &x, const double &y, const double &phi, const double &stdXY, const double &stdPhi) const |
Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window. | |
void | saveParzenPDFToTextFile (const char *fileName, const double &x_min, const double &x_max, const double &y_min, const double &y_max, const double &phi, const double &stepSizeXY, const double &stdXY, const double &stdPhi) const |
Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window. | |
RTTI stuff | |
typedef CPosePDFParticlesPtr | SmartPtr |
static mrpt::utils::CLASSINIT | _init_CPosePDFParticles |
static mrpt::utils::TRuntimeClassId | classCPosePDFParticles |
static const mrpt::utils::TRuntimeClassId * | classinfo |
static const mrpt::utils::TRuntimeClassId * | _GetBaseClass () |
virtual const mrpt::utils::TRuntimeClassId * | GetRuntimeClass () const |
Returns information about the class of an object in runtime. | |
virtual mrpt::utils::CObject * | duplicate () const |
Returns a copy of the object, indepently of its class. | |
static mrpt::utils::CObject * | CreateObject () |
static CPosePDFParticlesPtr | Create () |
A typedef for the associated smart pointer
Definition at line 59 of file CPosePDFParticles.h.
mrpt::poses::CPosePDFParticles::CPosePDFParticles | ( | size_t | M = 1 | ) |
Constructor.
M | The number of m_particles. |
mrpt::poses::CPosePDFParticles::CPosePDFParticles | ( | const CPosePDFParticles & | obj | ) | [inline] |
Copy constructor:
Definition at line 76 of file CPosePDFParticles.h.
virtual mrpt::poses::CPosePDFParticles::~CPosePDFParticles | ( | ) | [virtual] |
Destructor.
static const mrpt::utils::TRuntimeClassId* mrpt::poses::CPosePDFParticles::_GetBaseClass | ( | ) | [static, protected] |
Reimplemented from mrpt::poses::CPosePDF.
void mrpt::poses::CPosePDFParticles::append | ( | CPosePDFParticles & | o | ) |
Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
void mrpt::poses::CPosePDFParticles::bayesianFusion | ( | const CPosePDF & | p1, |
const CPosePDF & | p2, | ||
const double & | minMahalanobisDistToDrop = 0 |
||
) | [virtual] |
Bayesian fusion.
Implements mrpt::poses::CPosePDF.
void mrpt::poses::CPosePDFParticles::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< CPose2D, 3 >.
void mrpt::poses::CPosePDFParticles::clear | ( | ) |
Free all the memory associated to m_particles, and set the number of parts = 0.
void mrpt::poses::CPosePDFParticles::copyFrom | ( | const CPosePDF & | o | ) | [virtual] |
Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
Implements mrpt::poses::CPosePDF.
static CPosePDFParticlesPtr mrpt::poses::CPosePDFParticles::Create | ( | ) | [static] |
static mrpt::utils::CObject* mrpt::poses::CPosePDFParticles::CreateObject | ( | ) | [static] |
void mrpt::poses::CPosePDFParticles::drawSingleSample | ( | CPose2D & | outPart | ) | const [virtual] |
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
Implements mrpt::utils::CProbabilityDensityFunction< CPose2D, 3 >.
virtual mrpt::utils::CObject* mrpt::poses::CPosePDFParticles::duplicate | ( | ) | const [virtual] |
Returns a copy of the object, indepently of its class.
Implements mrpt::utils::CObject.
double mrpt::poses::CPosePDFParticles::evaluatePDF_parzen | ( | const double & | x, |
const double & | y, | ||
const double & | phi, | ||
const double & | stdXY, | ||
const double & | stdPhi | ||
) | const |
Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window.
void mrpt::poses::CPosePDFParticles::getCovarianceAndMean | ( | CMatrixDouble33 & | cov, |
CPose2D & | mean_point | ||
) | const |
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
void mrpt::poses::CPosePDFParticles::getMean | ( | CPose2D & | mean_pose | ) | const [virtual] |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
Implements mrpt::utils::CProbabilityDensityFunction< CPose2D, 3 >.
CPose2D mrpt::poses::CPosePDFParticles::getMostLikelyParticle | ( | ) | const |
Returns the particle with the highest weight.
Reimplemented from mrpt::bayes::CParticleFilterData< CPose2D >.
CPose2D mrpt::poses::CPosePDFParticles::getParticlePose | ( | size_t | i | ) | const |
Returns the pose of the i'th particle.
virtual const mrpt::utils::TRuntimeClassId* mrpt::poses::CPosePDFParticles::GetRuntimeClass | ( | ) | const [virtual] |
Returns information about the class of an object in runtime.
Reimplemented from mrpt::poses::CPosePDF.
mrpt::poses::CPosePDFParticles::IMPLEMENT_PARTICLE_FILTER_CAPABLE | ( | CPose2D | ) |
void mrpt::poses::CPosePDFParticles::inverse | ( | CPosePDF & | o | ) | const [virtual] |
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Implements mrpt::poses::CPosePDF.
void mrpt::poses::CPosePDFParticles::operator+= | ( | const CPose2D & | Ap | ) |
Appends (pose-composition) a given pose "p" to each particle.
void mrpt::poses::CPosePDFParticles::performSubstitution | ( | std::vector< int > & | indx | ) |
Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly.
void mrpt::poses::CPosePDFParticles::resetDeterministic | ( | const CPose2D & | location, |
size_t | particlesCount = 0 |
||
) |
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
location | The location to set all the m_particles. |
particlesCount | If this is set to 0 the number of m_particles remains unchanged. |
void mrpt::poses::CPosePDFParticles::resetUniform | ( | const double & | x_min, |
const double & | x_max, | ||
const double & | y_min, | ||
const double & | y_max, | ||
const double & | phi_min = -M_PI , |
||
const double & | phi_max = M_PI , |
||
const int & | particlesCount = -1 |
||
) |
Reset the PDF to an uniformly distributed one, inside of the defined cube.
If particlesCount is set to -1 the number of m_particles remains unchanged.
void mrpt::poses::CPosePDFParticles::saveParzenPDFToTextFile | ( | const char * | fileName, |
const double & | x_min, | ||
const double & | x_max, | ||
const double & | y_min, | ||
const double & | y_max, | ||
const double & | phi, | ||
const double & | stepSizeXY, | ||
const double & | stdXY, | ||
const double & | stdPhi | ||
) | const |
Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window.
void mrpt::poses::CPosePDFParticles::saveToTextFile | ( | const std::string & | file | ) | const [virtual] |
Save PDF's m_particles to a text file.
In each line it will go: "x y phi weight"
Implements mrpt::utils::CProbabilityDensityFunction< CPose2D, 3 >.
size_t mrpt::poses::CPosePDFParticles::size | ( | ) | const [inline] |
Get the m_particles count (equivalent to "particlesCount")
Definition at line 131 of file CPosePDFParticles.h.
mrpt::utils::CLASSINIT mrpt::poses::CPosePDFParticles::_init_CPosePDFParticles [static, protected] |
Definition at line 59 of file CPosePDFParticles.h.
Definition at line 59 of file CPosePDFParticles.h.
Definition at line 59 of file CPosePDFParticles.h.
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011 |