#include <mrpt/slam/CMultiMetricMapPDF.h>
Classes | |
struct | TPathBin |
Auxiliary structure. More... | |
struct | TPredictionParams |
The struct for passing extra simulation parameters to the prediction/update stage when running a particle filter. More... | |
Public Member Functions | |
CMultiMetricMapPDF (const bayes::CParticleFilter::TParticleFilterOptions &opts=bayes::CParticleFilter::TParticleFilterOptions(), const mrpt::slam::TSetOfMetricMapInitializers *mapsInitializers=NULL, const TPredictionParams *predictionOptions=NULL) | |
Constructor. | |
virtual | ~CMultiMetricMapPDF () |
Destructor. | |
void | clear (const CPose2D &initialPose) |
Clear all elements of the maps, and restore all paths to a single starting pose. | |
void | clear (const CPose3D &initialPose) |
Clear all elements of the maps, and restore all paths to a single starting pose. | |
void | getEstimatedPosePDFAtTime (size_t timeStep, CPose3DPDFParticles &out_estimation) const |
Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep", from 0 to N-1. | |
void | getEstimatedPosePDF (CPose3DPDFParticles &out_estimation) const |
Returns the current estimate of the robot pose, as a particles PDF. | |
CMultiMetricMap * | getCurrentMetricMapEstimation () |
Returns the weighted averaged map based on the current best estimation. | |
CMultiMetricMap * | getCurrentMostLikelyMetricMap () |
Returns a pointer to the current most likely map (associated to the most likely particle). | |
void | insertObservation (CSensoryFrame &sf) |
Insert an observation to the map, at each particle's pose and to each particle's metric map. | |
void | getPath (size_t i, std::deque< math::TPose3D > &out_path) const |
Return the path (in absolute coordinate poses) for the i'th particle. | |
double | getCurrentEntropyOfPaths () |
Returns the current entropy of paths, computed as the average entropy of poses along the path, where entropy of each pose estimation is computed as the entropy of the gaussian approximation covariance. | |
double | getCurrentJointEntropy () |
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Exploration Using" by C. | |
void | updateSensoryFrameSequence () |
Update the poses estimation of the member "SFs" according to the current path belief. | |
void | saveCurrentPathEstimationToTextFile (const std::string &fil) |
A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively). | |
Public Attributes | |
mrpt::slam::CMultiMetricMapPDF::TPredictionParams | options |
The struct for passing extra simulation parameters to the prediction/update stage when running a particle filter. | |
float | newInfoIndex |
An index [0,1] measuring how much information an observation aports to the map (Typ. | |
Protected Member Functions | |
void | prediction_and_update_pfStandardProposal (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
The PF algorithm implementation. | |
void | prediction_and_update_pfOptimalProposal (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
The PF algorithm implementation. | |
void | prediction_and_update_pfAuxiliaryPFOptimal (const mrpt::slam::CActionCollection *action, const mrpt::slam::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) |
The PF algorithm implementation. | |
Private Member Functions | |
IMPLEMENT_PARTICLE_FILTER_CAPABLE (CRBPFParticleData) | |
float | H (float p) |
Entropy aux. | |
const TPose3D * | getLastPose (const size_t &i) const |
Internally used: It returns a pointer to the last robot pose in the path for the i'th particle (or NULL if path is empty). | |
void | rebuildAverageMap () |
Rebuild the "expected" grid map. | |
void | loadTPathBinFromPath (CMultiMetricMapPDF::TPathBin &outBin, std::deque< TPose3D > *path=NULL, CPose3D *newPose=NULL) |
Fills out a "TPathBin" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options". | |
int | findTPathBinIntoSet (TPathBin &desiredBin, std::deque< TPathBin > &theSet) |
Checks if a given "TPathBin" element is already into a set of them, and return its index (first one is 0), or -1 if not found. | |
Static Private Member Functions | |
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 CPose3D *x) |
Auxiliary function that evaluates the likelihood of an observation, given a robot pose, and according to the options in "CPosePDFParticles::options". | |
Private Attributes | |
CMultiMetricMap | averageMap |
Internal buffer for the averaged map. | |
bool | averageMapIsUpdated |
CSensFrameProbSequence | SFs |
The SFs and their corresponding pose estimations:. | |
std::vector< uint32_t > | SF2robotPath |
A mapping between indexes in the SFs to indexes in the robot paths from particles. | |
vector_double | m_pfAuxiliaryPFOptimal_estimatedProb |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
vector_double | m_maxLikelihood |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
CPoseRandomSampler | m_movementDrawer |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
std::vector< CPose2D > | m_movementDrawMaximumLikelihood |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm. | |
Friends | |
class | CMetricMapBuilderRBPF |
This class is used internally by the map building algorithm in "mrpt::slam::CMetricMapBuilderRBPF"
Definition at line 72 of file CMultiMetricMapPDF.h.
mrpt::slam::CMultiMetricMapPDF::CMultiMetricMapPDF | ( | const bayes::CParticleFilter::TParticleFilterOptions & | opts = bayes::CParticleFilter::TParticleFilterOptions() , |
|
const mrpt::slam::TSetOfMetricMapInitializers * | mapsInitializers = NULL , |
|||
const TPredictionParams * | predictionOptions = NULL | |||
) |
Constructor.
virtual mrpt::slam::CMultiMetricMapPDF::~CMultiMetricMapPDF | ( | ) | [virtual] |
Destructor.
static double mrpt::slam::CMultiMetricMapPDF::auxiliarComputeObservationLikelihood | ( | const bayes::CParticleFilter::TParticleFilterOptions & | PF_options, | |
const CParticleFilterCapable * | obj, | |||
size_t | particleIndexForMap, | |||
const CSensoryFrame * | observation, | |||
const CPose3D * | 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::slam::CMultiMetricMapPDF::clear | ( | const CPose3D & | initialPose | ) |
Clear all elements of the maps, and restore all paths to a single starting pose.
void mrpt::slam::CMultiMetricMapPDF::clear | ( | const CPose2D & | initialPose | ) |
Clear all elements of the maps, and restore all paths to a single starting pose.
int mrpt::slam::CMultiMetricMapPDF::findTPathBinIntoSet | ( | TPathBin & | desiredBin, | |
std::deque< TPathBin > & | theSet | |||
) | [private] |
Checks if a given "TPathBin" element is already into a set of them, and return its index (first one is 0), or -1 if not found.
double mrpt::slam::CMultiMetricMapPDF::getCurrentEntropyOfPaths | ( | ) |
Returns the current entropy of paths, computed as the average entropy of poses along the path, where entropy of each pose estimation is computed as the entropy of the gaussian approximation covariance.
double mrpt::slam::CMultiMetricMapPDF::getCurrentJointEntropy | ( | ) |
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Exploration Using" by C.
Stachniss, G. Grissetti and W.Burgard.
CMultiMetricMap* mrpt::slam::CMultiMetricMapPDF::getCurrentMetricMapEstimation | ( | ) |
Returns the weighted averaged map based on the current best estimation.
If you need a persistent copy of this object, please use "CSerializable::duplicate" and use the copy.
CMultiMetricMap* mrpt::slam::CMultiMetricMapPDF::getCurrentMostLikelyMetricMap | ( | ) |
Returns a pointer to the current most likely map (associated to the most likely particle).
void mrpt::slam::CMultiMetricMapPDF::getEstimatedPosePDF | ( | CPose3DPDFParticles & | out_estimation | ) | const |
Returns the current estimate of the robot pose, as a particles PDF.
void mrpt::slam::CMultiMetricMapPDF::getEstimatedPosePDFAtTime | ( | size_t | timeStep, | |
CPose3DPDFParticles & | out_estimation | |||
) | const |
Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep", from 0 to N-1.
const TPose3D* mrpt::slam::CMultiMetricMapPDF::getLastPose | ( | const size_t & | i | ) | const [private] |
Internally used: It returns a pointer to the last robot pose in the path for the i'th particle (or NULL if path is empty).
void mrpt::slam::CMultiMetricMapPDF::getPath | ( | size_t | i, | |
std::deque< math::TPose3D > & | out_path | |||
) | const |
Return the path (in absolute coordinate poses) for the i'th particle.
On | index out of bounds |
float mrpt::slam::CMultiMetricMapPDF::H | ( | float | p | ) | [private] |
Entropy aux.
function
mrpt::slam::CMultiMetricMapPDF::IMPLEMENT_PARTICLE_FILTER_CAPABLE | ( | CRBPFParticleData | ) | [private] |
void mrpt::slam::CMultiMetricMapPDF::insertObservation | ( | CSensoryFrame & | sf | ) |
Insert an observation to the map, at each particle's pose and to each particle's metric map.
sf | The SF to be inserted |
void mrpt::slam::CMultiMetricMapPDF::loadTPathBinFromPath | ( | CMultiMetricMapPDF::TPathBin & | outBin, | |
std::deque< TPose3D > * | path = NULL , |
|||
CPose3D * | newPose = NULL | |||
) | [private] |
Fills out a "TPathBin" variable, given a path hypotesis and (if not set to NULL) a new pose appended at the end, using the KLD params in "options".
static double mrpt::slam::CMultiMetricMapPDF::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".
void mrpt::slam::CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFOptimal | ( | const mrpt::slam::CActionCollection * | action, | |
const mrpt::slam::CSensoryFrame * | observation, | |||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | |||
) | [protected, virtual] |
void mrpt::slam::CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal | ( | const mrpt::slam::CActionCollection * | action, | |
const mrpt::slam::CSensoryFrame * | observation, | |||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | |||
) | [protected, virtual] |
void mrpt::slam::CMultiMetricMapPDF::prediction_and_update_pfStandardProposal | ( | const mrpt::slam::CActionCollection * | action, | |
const mrpt::slam::CSensoryFrame * | observation, | |||
const bayes::CParticleFilter::TParticleFilterOptions & | PF_options | |||
) | [protected, virtual] |
void mrpt::slam::CMultiMetricMapPDF::rebuildAverageMap | ( | ) | [private] |
Rebuild the "expected" grid map.
Used internally, do not call
void mrpt::slam::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile | ( | const std::string & | fil | ) |
A logging utility: saves the current path estimation for each particle in a text file (a row per particle, each 3-column-entry is a set [x,y,phi], respectively).
void mrpt::slam::CMultiMetricMapPDF::updateSensoryFrameSequence | ( | ) |
Update the poses estimation of the member "SFs" according to the current path belief.
friend class CMetricMapBuilderRBPF [friend] |
Definition at line 74 of file CMultiMetricMapPDF.h.
bool mrpt::slam::CMultiMetricMapPDF::averageMapIsUpdated [private] |
Definition at line 109 of file CMultiMetricMapPDF.h.
vector_double mrpt::slam::CMultiMetricMapPDF::m_maxLikelihood [mutable, private] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 280 of file CMultiMetricMapPDF.h.
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 282 of file CMultiMetricMapPDF.h.
std::vector<CPose2D> mrpt::slam::CMultiMetricMapPDF::m_movementDrawMaximumLikelihood [mutable, private] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 286 of file CMultiMetricMapPDF.h.
vector_double mrpt::slam::CMultiMetricMapPDF::m_pfAuxiliaryPFOptimal_estimatedProb [mutable, private] |
Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.
Definition at line 276 of file CMultiMetricMapPDF.h.
An index [0,1] measuring how much information an observation aports to the map (Typ.
threshold=0.07)
Definition at line 234 of file CMultiMetricMapPDF.h.
The struct for passing extra simulation parameters to the prediction/update stage when running a particle filter.
std::vector<uint32_t> mrpt::slam::CMultiMetricMapPDF::SF2robotPath [private] |
A mapping between indexes in the SFs to indexes in the robot paths from particles.
Definition at line 117 of file CMultiMetricMapPDF.h.
The SFs and their corresponding pose estimations:.
Definition at line 113 of file CMultiMetricMapPDF.h.
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:20:53 EDT 2009 |