MRPT logo

mrpt::poses::CPosePDFParticles Class Reference

Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples. More...

#include <mrpt/poses/CPosePDFParticles.h>

Inheritance diagram for mrpt::poses::CPosePDFParticles:

mrpt::poses::CPosePDF mrpt::bayes::CParticleFilterCapable mrpt::bayes::CParticleFilterData< CPose2D > mrpt::utils::CSerializable

List of all members.

Classes

struct  lt_TPoseBin
 Auxiliary structure. More...
struct  TPoseBin
 Auxiliary structure. More...
struct  TPredictionParams
 The struct for passing extra simulation parameters to the prediction stage when running a particle filter. More...

Public Member Functions

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 resetUniformFreeSpace (COccupancyGridMap2D *theMap, const double &freeCellsThreshold=0.7, const int &particlesCount=-1, const double &x_min=-1e10f, const double &x_max=1e10f, const double &y_min=-1e10f, const double &y_max=1e10f, const double &phi_min=-M_PI, const double &phi_max=M_PI)
 Reset the PDF to an uniformly distributed one, but only in the free-space of a given 2D occupancy-grid-map.
CPose2D getEstimatedPose () const
 Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
CMatrixD getEstimatedCovariance () const
 Returns an estimate of the pose covariance matrix (3x3 cov.matrix for x,y,phi variables).
CPose2D getParticlePose (size_t i) const
 Returns the pose of the i'th particle.
void prediction_and_update_pfStandardProposal (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
 Update the m_particles, predicting the posterior of robot pose and map after a movement command.
void prediction_and_update_pfAuxiliaryPFStandard (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
 Update the m_particles, predicting the posterior of robot pose and map after a movement command.
void prediction_and_update_pfAuxiliaryPFOptimal (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
 Update the m_particles, predicting the posterior of robot pose and map after a movement command.
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 CPose2D &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 drawManySamples (size_t N, std::vector< vector_double > &outSamples) const
 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.
void operator+= (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 (CPosePDF &p1, 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.

Public Attributes

struct MRPTDLLIMPEXP
mrpt::poses::CPosePDFParticles::TPredictionParams 
options
 The struct for passing extra simulation parameters to the prediction stage when running a particle filter.

Private Member Functions

 IMPLEMENT_PARTICLE_FILTER_CAPABLE (CPose2D)

Static Private Member Functions

static double particlesEvaluator_AuxPFStandard (const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
 Auxiliary function used in "prediction_and_update_pfAuxiliaryPFStandard".
static double particlesEvaluator_AuxPFOptimal (const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t index, const void *action, const void *observation)
 Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".
static double auxiliarComputeObservationLikelihood (const bayes::CParticleFilter::TParticleFilterOptions &PF_options, const CParticleFilterCapable *obj, size_t particleIndexForMap, const CSensoryFrame *observation, const CPose2D *x)
 Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options".

Private Attributes

vector_double m_pfAuxiliaryPFOptimal_estimatedProb
 Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
vector_double m_pfAuxiliaryPFStandard_estimatedProb
 Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
CPoseRandomSampler m_movementDrawer
 Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.


Detailed Description

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 also implements particle filtering for robot localization. See the MRPT application "app/pf-localization" for an example of usage.

See also:
CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable

Definition at line 60 of file CPosePDFParticles.h.


Constructor & Destructor Documentation

mrpt::poses::CPosePDFParticles::CPosePDFParticles ( size_t  M = 1  ) 

Constructor.

Parameters:
M The number of m_particles.

mrpt::poses::CPosePDFParticles::CPosePDFParticles ( const CPosePDFParticles obj  )  [inline]

Copy constructor:.

Definition at line 120 of file CPosePDFParticles.h.

virtual mrpt::poses::CPosePDFParticles::~CPosePDFParticles (  )  [virtual]

Destructor.


Member Function Documentation

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.

static double mrpt::poses::CPosePDFParticles::auxiliarComputeObservationLikelihood ( const bayes::CParticleFilter::TParticleFilterOptions PF_options,
const CParticleFilterCapable obj,
size_t  particleIndexForMap,
const CSensoryFrame observation,
const CPose2D x 
) [static, private]

Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options".

void mrpt::poses::CPosePDFParticles::bayesianFusion ( CPosePDF p1,
CPosePDF p2,
const double &  minMahalanobisDistToDrop = 0 
) [virtual]

Bayesian fusion.

Implements mrpt::poses::CPosePDF.

void mrpt::poses::CPosePDFParticles::changeCoordinatesReference ( const CPose2D 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::CPosePDF.

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.

void mrpt::poses::CPosePDFParticles::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 1x3 vectors, where each row contains a (x,y,phi) datum.

Implements mrpt::poses::CPosePDF.

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::poses::CPosePDF.

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.

See also:
saveParzenPDFToTextFile

CMatrixD mrpt::poses::CPosePDFParticles::getEstimatedCovariance (  )  const [virtual]

Returns an estimate of the pose covariance matrix (3x3 cov.matrix for x,y,phi variables).

Implements mrpt::poses::CPosePDF.

CPose2D mrpt::poses::CPosePDFParticles::getEstimatedPose (  )  const [virtual]

Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.

Implements mrpt::poses::CPosePDF.

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.

mrpt::poses::CPosePDFParticles::IMPLEMENT_PARTICLE_FILTER_CAPABLE ( CPose2D   )  [private]

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+= ( CPose2D  Ap  ) 

Appends (pose-composition) a given pose "p" to each particle.

static double mrpt::poses::CPosePDFParticles::particlesEvaluator_AuxPFOptimal ( const bayes::CParticleFilter::TParticleFilterOptions PF_options,
const CParticleFilterCapable obj,
size_t  index,
const void *  action,
const void *  observation 
) [static, private]

Auxiliary function used in "prediction_and_update_pfAuxiliaryPFOptimal".

static double mrpt::poses::CPosePDFParticles::particlesEvaluator_AuxPFStandard ( const bayes::CParticleFilter::TParticleFilterOptions PF_options,
const CParticleFilterCapable obj,
size_t  index,
const void *  action,
const void *  observation 
) [static, private]

Auxiliary function used in "prediction_and_update_pfAuxiliaryPFStandard".

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::prediction_and_update_pfAuxiliaryPFOptimal ( const mrpt::slam::CActionCollection action,
const mrpt::slam::CSensoryFrame observation,
const bayes::CParticleFilter::TParticleFilterOptions PF_options 
) [virtual]

Update the m_particles, predicting the posterior of robot pose and map after a movement command.

This method has additional configuration parameters in "options". Performs the update stage of the RBPF, using the sensed Sensorial Frame:

Parameters:
Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
See also:
options

Reimplemented from mrpt::bayes::CParticleFilterCapable.

void mrpt::poses::CPosePDFParticles::prediction_and_update_pfAuxiliaryPFStandard ( const mrpt::slam::CActionCollection action,
const mrpt::slam::CSensoryFrame observation,
const bayes::CParticleFilter::TParticleFilterOptions PF_options 
) [virtual]

Update the m_particles, predicting the posterior of robot pose and map after a movement command.

This method has additional configuration parameters in "options". Performs the update stage of the RBPF, using the sensed Sensorial Frame:

Parameters:
Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
See also:
options

Reimplemented from mrpt::bayes::CParticleFilterCapable.

void mrpt::poses::CPosePDFParticles::prediction_and_update_pfStandardProposal ( const mrpt::slam::CActionCollection action,
const mrpt::slam::CSensoryFrame observation,
const bayes::CParticleFilter::TParticleFilterOptions PF_options 
) [virtual]

Update the m_particles, predicting the posterior of robot pose and map after a movement command.

This method has additional configuration parameters in "options". Performs the update stage of the RBPF, using the sensed Sensorial Frame:

Parameters:
action This is a pointer to CActionCollection, containing the pose change the robot has been commanded.
observation This must be a pointer to a CSensoryFrame object, with robot sensed observations.
See also:
options

Reimplemented from mrpt::bayes::CParticleFilterCapable.

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.

Parameters:
location The location to set all the m_particles.
particlesCount If this is set to 0 the number of m_particles remains unchanged.
See also:
resetUniform, resetUniformFreeSpace

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.

See also:
resetDeterministic, resetUniformFreeSpace

void mrpt::poses::CPosePDFParticles::resetUniformFreeSpace ( COccupancyGridMap2D theMap,
const double &  freeCellsThreshold = 0.7,
const int &  particlesCount = -1,
const double &  x_min = -1e10f,
const double &  x_max = 1e10f,
const double &  y_min = -1e10f,
const double &  y_max = 1e10f,
const double &  phi_min = -M_PI,
const double &  phi_max = M_PI 
)

Reset the PDF to an uniformly distributed one, but only in the free-space of a given 2D occupancy-grid-map.

Orientation is randomly generated in the whole 2*PI range.

Parameters:
theMap The occupancy grid map
freeCellsThreshold The minimum free-probability to consider a cell as empty (default is 0.7)
particlesCount If set to -1 the number of m_particles remains unchanged.
x_min The limits of the area to look for free cells.
x_max The limits of the area to look for free cells.
y_min The limits of the area to look for free cells.
y_max The limits of the area to look for free cells.
phi_min The limits of the area to look for free cells.
phi_max The limits of the area to look for free cells.
See also:
resetDeterm32inistic
Exceptions:
std::exception On any error (no free cell found in map, map=NULL, etc...)

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.

See also:
evaluatePDF_parzen

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::poses::CPosePDF.

size_t mrpt::poses::CPosePDFParticles::size (  )  const [inline]

Get the m_particles count (equivalent to "particlesCount").

Definition at line 247 of file CPosePDFParticles.h.


Member Data Documentation

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 337 of file CPosePDFParticles.h.

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 329 of file CPosePDFParticles.h.

Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.

Definition at line 333 of file CPosePDFParticles.h.

The struct for passing extra simulation parameters to the prediction stage when running a particle filter.

See also:
prediction




Page generated by Doxygen 1.5.9 for MRPT 0.6.5 SVN: at Sun Aug 2 11:41:17 CDT 2009