9#ifndef CMultiMetricMapPDF_H
10#define CMultiMetricMapPDF_H
29namespace slam {
class CMetricMapBuilderRBPF; }
43 mapTillNow( mapsInitializers ),
63 public
mrpt::utils::CSerializable,
202 void getPath(
size_t i, std::deque<math::TPose3D> &out_path)
const;
254 const size_t particleIndexForMap,
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
CRBPFParticleData CParticleDataContent
This is the type inside the corresponding CParticleData class.
This class stores any customizable set of metric maps.
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
double PF_SLAM_computeObservationLikelihoodForParticle(const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options, const size_t particleIndexForMap, const mrpt::obs::CSensoryFrame &observation, const mrpt::poses::CPose3D &x) const
Evaluate the observation likelihood for one particle at a given location.
void rebuildAverageMap()
Rebuild the "expected" grid map.
void prediction_and_update_pfStandardProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
CMultiMetricMap * getCurrentMostLikelyMetricMap()
Returns a pointer to the current most likely map (associated to the most likely particle).
void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
CMultiMetricMap * getCurrentMetricMapEstimation()
Returns the weighted averaged map based on the current best estimation.
CMultiMetricMapPDF(const bayes::CParticleFilter::TParticleFilterOptions &opts=bayes::CParticleFilter::TParticleFilterOptions(), const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers=NULL, const TPredictionParams *predictionOptions=NULL)
Constructor.
void getPath(size_t i, std::deque< math::TPose3D > &out_path) const
Return the path (in absolute coordinate poses) for the i'th particle.
void getEstimatedPosePDFAtTime(size_t timeStep, mrpt::poses::CPose3DPDFParticles &out_estimation) const
Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep",...
double getCurrentEntropyOfPaths()
Returns the current entropy of paths, computed as the average entropy of poses along the path,...
mrpt::maps::CSimpleMap SFs
The SFs and their corresponding pose estimations:
const mrpt::math::TPose3D * getLastPose(const size_t i) const
Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty).
mrpt::maps::CMultiMetricMap averageMap
Internal buffer for the averaged map.
void prediction_and_update_pfOptimalProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
The PF algorithm implementation.
void clear(const mrpt::poses::CPose3D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
float H(float p)
Entropy aux.
double getCurrentJointEntropy()
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Explora...
void clear(const mrpt::poses::CPose2D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
void PF_SLAM_implementation_custom_update_particle_with_new_pose(CParticleDataContent *particleData, const mrpt::math::TPose3D &newPose) const
virtual ~CMultiMetricMapPDF()
Destructor.
bool PF_SLAM_implementation_doWeHaveValidObservations(const CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const
float newInfoIndex
An index [0,1] measuring how much information an observation aports to the map (Typ.
void updateSensoryFrameSequence()
Update the poses estimation of the member "SFs" according to the current path belief.
void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles &out_estimation) const
Returns the current estimate of the robot pose, as a particles PDF.
void insertObservation(mrpt::obs::CSensoryFrame &sf)
Insert an observation to the map, at each particle's pose and to each particle's metric map.
bool PF_SLAM_implementation_skipRobotMovement() const
Make a specialization if needed, eg.
void saveCurrentPathEstimationToTextFile(const std::string &fil)
A logging utility: saves the current path estimation for each particle in a text file (a row per part...
std::vector< uint32_t > SF2robotPath
A mapping between indexes in the SFs to indexes in the robot paths from particles.
size_t getNumberOfObservationsInSimplemap() const
Get the number of CSensoryFrame inserted into the internal member SFs.
With this struct options are provided to the observation likelihood computation process.
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
CMultiMetricMap mapTillNow
CRBPFParticleData(const TSetOfMetricMapInitializers *mapsInitializers=NULL)
std::deque< mrpt::math::TPose3D > robotPath
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
size_t size() const
Returns the count of pairs (pose,sensory data)
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Declares a class for storing a collection of robot actions.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
A class used to store a 2D pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
The ICP algorithm configuration data.
This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM).
Option set for KLD algorithm.
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
The virtual base class which provides a unified interface for all persistent objects in MRPT.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The configuration of a particle filter.
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
TPredictionParams()
Default settings method.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
float ICPGlobalAlign_MinQuality
[prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum ...
COccupancyGridMap2D::TLikelihoodOptions update_gridMapLikelihoodOptions
[update stage] If the likelihood is computed through the occupancy grid map, then this structure is p...
int pfOptimalProposal_mapSelection
[pf optimal proposal only] Only for PF algorithm=2 (Exact "pfOptimalProposal") Select the map on whic...
mrpt::slam::CICP::TConfigParams icp_params
ICP parameters, used only when "PF_algorithm=2" in the particle filter.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::slam::TKLDParams KLD_params
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).