MRPT logo

mrpt::slam::CMultiMetricMapPDF Class Reference

Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (This class is the base of RBPF-SLAM applications). More...

#include <mrpt/slam/CMultiMetricMapPDF.h>

Inheritance diagram for mrpt::slam::CMultiMetricMapPDF:

mrpt::utils::CSerializable mrpt::bayes::CParticleFilterCapable mrpt::bayes::CParticleFilterData< CRBPFParticleData >

List of all members.

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.
CMultiMetricMapgetCurrentMetricMapEstimation ()
 Returns the weighted averaged map based on the current best estimation.
CMultiMetricMapgetCurrentMostLikelyMetricMap ()
 Returns a pointer to the current most likely map (associated to the most likely particle).
void insertObservation (CSensoryFrame &sf, bool forceInsertion=true)
 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< CPose3D > &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 CPose3DgetLastPose (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< CPose3D > *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< CPose2Dm_movementDrawMaximumLikelihood
 Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Friends

class CMetricMapBuilderRBPF


Detailed Description

Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (This class is the base of RBPF-SLAM applications).

This class is used internally by the map building algorithm in "mrpt::slam::CMetricMapBuilderRBPF"

See also:
mrpt::slam::CMetricMapBuilderRBPF

Definition at line 72 of file CMultiMetricMapPDF.h.


Constructor & Destructor Documentation

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.


Member Function Documentation

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.

See also:
getEstimatedPosePDFAtTime

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.

See also:
getEstimatedPosePDF

const CPose3D* 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< CPose3D > &  out_path 
) const

Return the path (in absolute coordinate poses) for the i'th particle.

Exceptions:
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,
bool  forceInsertion = true 
)

Insert an observation to the map, at each particle's pose and to each particle's metric map.

Parameters:
sf The SF to be inserted
forceInsertion If set to true (default) the SF is always inserted. If set to false, each map will check if the observation apports enought new information as to be inserted.

void mrpt::slam::CMultiMetricMapPDF::loadTPathBinFromPath ( CMultiMetricMapPDF::TPathBin outBin,
std::deque< CPose3D > *  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]

The PF algorithm implementation.

Reimplemented from mrpt::bayes::CParticleFilterCapable.

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]

The PF algorithm implementation.

Reimplemented from mrpt::bayes::CParticleFilterCapable.

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]

The PF algorithm implementation.

Reimplemented from mrpt::bayes::CParticleFilterCapable.

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.


Friends And Related Function Documentation

friend class CMetricMapBuilderRBPF [friend]

Definition at line 74 of file CMultiMetricMapPDF.h.


Member Data Documentation

Internal buffer for the averaged map.

Definition at line 108 of file CMultiMetricMapPDF.h.

Definition at line 109 of file CMultiMetricMapPDF.h.

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 282 of file CMultiMetricMapPDF.h.

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 284 of file CMultiMetricMapPDF.h.

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 288 of file CMultiMetricMapPDF.h.

Auxiliary variable used in the "pfAuxiliaryPFOptimal" algorithm.

Definition at line 278 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 236 of file CMultiMetricMapPDF.h.

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

See also:
prediction_and_update

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.8 for MRPT 0.6.5 SVN: at Thu Feb 26 02:19:01 EST 2009