9 #ifndef CMultiMetricMapPDF_H
10 #define CMultiMetricMapPDF_H
29 namespace slam {
class CMetricMapBuilderRBPF; }
43 mapTillNow( mapsInitializers ),
63 public
mrpt::utils::CSerializable,
65 public
mrpt::bayes::CParticleFilterDataImpl<CMultiMetricMapPDF,
mrpt::bayes::CParticleFilterData<CRBPFParticleData>::
CParticleList>,
66 public
mrpt::slam::PF_implementation<CRBPFParticleData,CMultiMetricMapPDF>
76 void prediction_and_update_pfStandardProposal(
77 const
mrpt::obs::CActionCollection * action,
78 const
mrpt::obs::CSensoryFrame * observation,
79 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
83 void prediction_and_update_pfOptimalProposal(
84 const
mrpt::obs::CActionCollection * action,
85 const
mrpt::obs::CSensoryFrame * observation,
86 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
90 void prediction_and_update_pfAuxiliaryPFOptimal(
91 const
mrpt::obs::CActionCollection * action,
92 const
mrpt::obs::CSensoryFrame * observation,
93 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
99 bool averageMapIsUpdated;
107 std::vector<uint32_t> SF2robotPath;
128 void loadFromConfigFile(
130 const std::string §ion);
171 virtual ~CMultiMetricMapPDF();
182 void getEstimatedPosePDFAtTime(
210 void getPath(
size_t i, std::deque<math::TPose3D> &out_path)
const;
214 double getCurrentEntropyOfPaths();
222 void updateSensoryFrameSequence();
235 void rebuildAverageMap();
246 void PF_SLAM_implementation_custom_update_particle_with_new_pose(
253 bool PF_SLAM_implementation_doWeHaveValidObservations(
254 const CParticleList &particles,
257 bool PF_SLAM_implementation_skipRobotMovement()
const;
260 double PF_SLAM_computeObservationLikelihoodForParticle(
262 const size_t particleIndexForMap,
The ICP algorithm configuration data.
This class stores a sequence of
pairs, thus a "metric map" can be t...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
mrpt::slam::CICP::TConfigParams icp_params
ICP parameters, used only when "PF_algorithm=2" in the particle filter.
float newInfoIndex
An index [0,1] measuring how much information an observation aports to the map (Typ.
float ICPGlobalAlign_MinQuality
[prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum ...
mrpt::maps::CMultiMetricMapPDF CMultiMetricMapPDF
Backward compatible typedef.
Option set for KLD algorithm.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
This class allows loading and storing values and vectors of different types from a configuration text...
CRBPFParticleData CParticleDataContent
This is the type inside the corresponding CParticleData class.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
#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...
COccupancyGridMap2D::TLikelihoodOptions update_gridMapLikelihoodOptions
[update stage] If the likelihood is computed through the occupancy grid map, then this structure is p...
With this struct options are provided to the observation likelihood computation process.
class BASE_IMPEXP CSerializable
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
void clear()
Clear all elements of the maps.
mrpt::slam::TKLDParams KLD_params
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
double getCurrentJointEntropy()
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...
size_t getNumberOfObservationsInSimplemap() const
Get the number of CSensoryFrame inserted into the internal member SFs.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
A class used to store a 2D pose.
This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM)...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
The configuration of a particle filter.
std::deque< mrpt::math::TPose3D > robotPath
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
int pfOptimalProposal_mapSelection
[pf optimal proposal only] Only for PF algorithm=2 (Exact "pfOptimalProposal") Select the map on whic...
This class stores any customizable set of metric maps.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
CMultiMetricMap mapTillNow
Declares a class that represents a Probability Density function (PDF) of a 3D pose.