#include <mrpt/poses/CPosePDFParticles.h>
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. | |
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 | 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 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 | 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. |
This class also implements particle filtering for robot localization. See the MRPT application "app/pf-localization" for an example of usage.
Definition at line 60 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] |
virtual mrpt::poses::CPosePDFParticles::~CPosePDFParticles | ( | ) | [virtual] |
Destructor.
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::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::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.
Reimplemented from mrpt::utils::CProbabilityDensityFunction< TDATA, STATE_LEN >.
void mrpt::poses::CPosePDFParticles::drawSingleSample | ( | CPose2D & | outPart | ) | const |
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!).
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 |
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
CPose2D mrpt::poses::CPosePDFParticles::getMostLikelyParticle | ( | ) | const |
Returns the particle with the highest weight.
Reimplemented from mrpt::bayes::CParticleFilterData< T >.
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] |
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:
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. |
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:
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. |
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:
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. |
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.
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::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.
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. |
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.
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< TDATA, STATE_LEN >.
size_t mrpt::poses::CPosePDFParticles::size | ( | ) | const [inline] |
Get the m_particles count (equivalent to "particlesCount").
Definition at line 251 of file CPosePDFParticles.h.
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 341 of file CPosePDFParticles.h.
vector_double mrpt::poses::CPosePDFParticles::m_pfAuxiliaryPFOptimal_estimatedProb [mutable, private] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 333 of file CPosePDFParticles.h.
vector_double mrpt::poses::CPosePDFParticles::m_pfAuxiliaryPFStandard_estimatedProb [mutable, private] |
Auxiliary variable used in the "pfAuxiliaryPFStandard" algorithm.
Definition at line 337 of file CPosePDFParticles.h.
struct MRPTDLLIMPEXP mrpt::poses::CPosePDFParticles::TPredictionParams mrpt::poses::CPosePDFParticles::options |
The struct for passing extra simulation parameters to the prediction stage when running a particle filter.
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 22:58:25 EDT 2009 |