Main MRPT website > C++ reference for MRPT 1.4.0
maps/CMultiMetricMapPDF.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CMultiMetricMapPDF_H
10 #define CMultiMetricMapPDF_H
11 
13 #include <mrpt/maps/CSimpleMap.h>
16 
18 
21 #include <mrpt/slam/CICP.h>
22 
24 
25 #include <mrpt/slam/link_pragmas.h>
26 
27 namespace mrpt
28 {
29 namespace slam { class CMetricMapBuilderRBPF; }
30 namespace maps
31 {
33 
34  /** Auxiliary class used in mrpt::maps::CMultiMetricMapPDF
35  * \ingroup mrpt_slam_grp
36  */
37  class SLAM_IMPEXP CRBPFParticleData : public mrpt::utils::CSerializable
38  {
39  // This must be added to any CSerializable derived class:
41  public:
42  CRBPFParticleData( const TSetOfMetricMapInitializers *mapsInitializers = NULL ) :
43  mapTillNow( mapsInitializers ),
44  robotPath()
45  {
46  }
47 
49  std::deque<mrpt::math::TPose3D> robotPath;
50  };
52 
53 
55 
56  /** 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).
57  * This class is used internally by the map building algorithm in "mrpt::slam::CMetricMapBuilderRBPF"
58  *
59  * \sa mrpt::slam::CMetricMapBuilderRBPF
60  * \ingroup metric_slam_grp
61  */
63  public mrpt::utils::CSerializable,
64  public mrpt::bayes::CParticleFilterData<CRBPFParticleData>,
65  public mrpt::bayes::CParticleFilterDataImpl<CMultiMetricMapPDF,mrpt::bayes::CParticleFilterData<CRBPFParticleData>::CParticleList>,
66  public mrpt::slam::PF_implementation<CRBPFParticleData,CMultiMetricMapPDF>
67  {
69 
70  // This must be added to any CSerializable derived class:
72 
73  protected:
74  /** The PF algorithm implementation.
75  */
77  const mrpt::obs::CActionCollection * action,
78  const mrpt::obs::CSensoryFrame * observation,
80 
81  /** The PF algorithm implementation.
82  */
84  const mrpt::obs::CActionCollection * action,
85  const mrpt::obs::CSensoryFrame * observation,
87 
88  /** The PF algorithm implementation.
89  */
91  const mrpt::obs::CActionCollection * action,
92  const mrpt::obs::CSensoryFrame * observation,
94 
95 
96  private:
97  /** Internal buffer for the averaged map. */
100 
101  /** The SFs and their corresponding pose estimations:
102  */
104 
105  /** A mapping between indexes in the SFs to indexes in the robot paths from particles.
106  */
107  std::vector<uint32_t> SF2robotPath;
108 
109 
110  /** Entropy aux. function
111  */
112  float H(float p);
113 
114  public:
115 
116  /** The struct for passing extra simulation parameters to the prediction/update stage
117  * when running a particle filter.
118  * \sa prediction_and_update
119  */
121  {
122  /** Default settings method */
124 
125  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
126  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
127 
128  /** [pf optimal proposal only] Only for PF algorithm=2 (Exact "pfOptimalProposal")
129  * Select the map on which to calculate the optimal
130  * proposal distribution. Values:
131  * 0: Gridmap -> Uses Scan matching-based approximation (based on Stachniss' work)
132  * 1: Landmarks -> Uses matching to approximate optimal
133  * 2: Beacons -> Used for exact optimal proposal in RO-SLAM
134  * 3: Pointsmap -> Uses Scan matching-based approximation with a map of points (based on Stachniss' work)
135  * Default = 0
136  */
138 
139 
140  /** [prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum quality ratio [0,1] of the alignment such as it will be accepted. Otherwise, raw odometry is used for those bad cases (default=0.7).
141  */
143 
144  /** [update stage] If the likelihood is computed through the occupancy grid map, then this structure is passed to the map when updating the particles weights in the update stage.
145  */
147 
149 
150  mrpt::slam::CICP::TConfigParams icp_params; //!< ICP parameters, used only when "PF_algorithm=2" in the particle filter.
151 
152  } options;
153 
154  /** Constructor
155  */
158  const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers = NULL,
159  const TPredictionParams *predictionOptions = NULL );
160 
161  /** Destructor
162  */
164 
165  /** Clear all elements of the maps, and restore all paths to a single starting pose */
166  void clear( const mrpt::poses::CPose2D &initialPose );
167 
168  /** Clear all elements of the maps, and restore all paths to a single starting pose */
169  void clear( const mrpt::poses::CPose3D &initialPose );
170 
171  /** Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep", from 0 to N-1.
172  * \sa getEstimatedPosePDF
173  */
175  size_t timeStep,
176  mrpt::poses::CPose3DPDFParticles &out_estimation ) const;
177 
178  /** Returns the current estimate of the robot pose, as a particles PDF.
179  * \sa getEstimatedPosePDFAtTime
180  */
182 
183  /** 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.
184  */
186 
187  /** Returns a pointer to the current most likely map (associated to the most likely particle).
188  */
190 
191  /** Get the number of CSensoryFrame inserted into the internal member SFs */
192  size_t getNumberOfObservationsInSimplemap() const { return SFs.size(); }
193 
194  /** Insert an observation to the map, at each particle's pose and to each particle's metric map.
195  * \param sf The SF to be inserted
196  */
198 
199  /** Return the path (in absolute coordinate poses) for the i'th particle.
200  * \exception On index out of bounds
201  */
202  void getPath(size_t i, std::deque<math::TPose3D> &out_path) const;
203 
204  /** 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.
205  */
207 
208  /** Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Exploration Using" by C. Stachniss, G. Grissetti and W.Burgard.
209  */
211 
212  /** Update the poses estimation of the member "SFs" according to the current path belief.
213  */
215 
216  /** 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).
217  */
218  void saveCurrentPathEstimationToTextFile( const std::string &fil );
219 
220  /** An index [0,1] measuring how much information an observation aports to the map (Typ. threshold=0.07)
221  */
223 
224  private:
225  /** Rebuild the "expected" grid map. Used internally, do not call
226  */
228 
229 
230 
231  public:
232  /** \name Virtual methods that the PF_implementations assume exist.
233  @{ */
234 
235  /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
236  const mrpt::math::TPose3D * getLastPose(const size_t i) const;
237 
239  CParticleDataContent *particleData,
240  const mrpt::math::TPose3D &newPose) const;
241 
242  // The base implementation is fine for this class.
243  // void PF_SLAM_implementation_replaceByNewParticleSet( ...
244 
246  const CParticleList &particles,
247  const mrpt::obs::CSensoryFrame *sf) const;
248 
250 
251  /** Evaluate the observation likelihood for one particle at a given location */
254  const size_t particleIndexForMap,
255  const mrpt::obs::CSensoryFrame &observation,
256  const mrpt::poses::CPose3D &x ) const;
257  /** @} */
258 
259 
260  }; // End of class def.
262 
263  } // End of namespace
264 } // End of namespace
265 
266 #endif
mrpt::maps::CMultiMetricMapPDF::getEstimatedPosePDF
void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles &out_estimation) const
Returns the current estimate of the robot pose, as a particles PDF.
mrpt::bayes::CParticleFilterData< CRBPFParticleData >::CParticleList
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
Definition: CParticleFilterData.h:182
mrpt::maps::CMultiMetricMapPDF::clear
void clear(const mrpt::poses::CPose3D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
mrpt::maps::CMultiMetricMapPDF::getLastPose
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).
CPoseRandomSampler.h
mrpt::maps::CMultiMetricMapPDF::TPredictionParams::pfOptimalProposal_mapSelection
int pfOptimalProposal_mapSelection
[pf optimal proposal only] Only for PF algorithm=2 (Exact "pfOptimalProposal") Select the map on whic...
Definition: maps/CMultiMetricMapPDF.h:137
mrpt::maps::CMultiMetricMapPDF::TPredictionParams::KLD_params
mrpt::slam::TKLDParams KLD_params
Definition: maps/CMultiMetricMapPDF.h:148
mrpt::maps::CRBPFParticleData
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
Definition: maps/CMultiMetricMapPDF.h:38
mrpt::slam::CICP::TConfigParams
The ICP algorithm configuration data.
Definition: CICP.h:58
mrpt::slam::TKLDParams
Option set for KLD algorithm.
Definition: TKLDParams.h:23
mrpt::maps::CMultiMetricMapPDF::clear
void clear(const mrpt::poses::CPose2D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
mrpt::maps::CMultiMetricMapPDF::getEstimatedPosePDFAtTime
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",...
mrpt::maps::CMultiMetricMapPDF::~CMultiMetricMapPDF
virtual ~CMultiMetricMapPDF()
Destructor.
mrpt::maps::CMultiMetricMapPDF::TPredictionParams::ICPGlobalAlign_MinQuality
float ICPGlobalAlign_MinQuality
[prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum ...
Definition: maps/CMultiMetricMapPDF.h:142
mrpt::slam::CMetricMapBuilderRBPF
This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM).
Definition: CMetricMapBuilderRBPF.h:47
CICP.h
CLoadableOptions.h
CSimpleMap.h
PF_implementations_data.h
mrpt::maps::CMultiMetricMapPDF::averageMap
mrpt::maps::CMultiMetricMap averageMap
Internal buffer for the averaged map.
Definition: maps/CMultiMetricMapPDF.h:98
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: obs/CActionCollection.h:31
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:17
mrpt::maps::CMultiMetricMapPDF::TPredictionParams::dumpToTextStream
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
mrpt::maps::CMultiMetricMapPDF::PF_SLAM_computeObservationLikelihoodForParticle
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.
mrpt::maps::CMultiMetricMapPDF::averageMapIsUpdated
bool averageMapIsUpdated
Definition: maps/CMultiMetricMapPDF.h:99
mrpt::maps::CRBPFParticleData::robotPath
std::deque< mrpt::math::TPose3D > robotPath
Definition: maps/CMultiMetricMapPDF.h:49
mrpt::maps::CMultiMetricMapPDF::SFs
mrpt::maps::CSimpleMap SFs
The SFs and their corresponding pose estimations:
Definition: maps/CMultiMetricMapPDF.h:103
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:72
mrpt::maps::CMultiMetricMapPDF::getCurrentMetricMapEstimation
CMultiMetricMap * getCurrentMetricMapEstimation()
Returns the weighted averaged map based on the current best estimation.
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: obs/CSensoryFrame.h:53
mrpt::maps::CMultiMetricMapPDF::H
float H(float p)
Entropy aux.
mrpt::maps::CMultiMetricMapPDF::updateSensoryFrameSequence
void updateSensoryFrameSequence()
Update the poses estimation of the member "SFs" according to the current path belief.
mrpt::maps::CMultiMetricMapPDF::TPredictionParams::icp_params
mrpt::slam::CICP::TConfigParams icp_params
ICP parameters, used only when "PF_algorithm=2" in the particle filter.
Definition: maps/CMultiMetricMapPDF.h:150
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
CMultiMetricMap.h
mrpt::maps::CMultiMetricMapPDF::getCurrentEntropyOfPaths
double getCurrentEntropyOfPaths()
Returns the current entropy of paths, computed as the average entropy of poses along the path,...
mrpt::utils::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: CConfigFileBase.h:31
mrpt::utils::CLoadableOptions
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
Definition: CLoadableOptions.h:32
mrpt::poses::CPose2D
A class used to store a 2D pose.
Definition: CPose2D.h:37
mrpt::maps::CMultiMetricMapPDF::getCurrentMostLikelyMetricMap
CMultiMetricMap * getCurrentMostLikelyMetricMap()
Returns a pointer to the current most likely map (associated to the most likely particle).
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: maps/CMultiMetricMap.h:123
mrpt::maps::CMultiMetricMapPDF::TPredictionParams::update_gridMapLikelihoodOptions
COccupancyGridMap2D::TLikelihoodOptions update_gridMapLikelihoodOptions
[update stage] If the likelihood is computed through the occupancy grid map, then this structure is p...
Definition: maps/CMultiMetricMapPDF.h:146
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
mrpt::maps::CSimpleMap::size
size_t size() const
Returns the count of pairs (pose,sensory data)
mrpt::maps::CMultiMetricMapPDF
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
Definition: maps/CMultiMetricMapPDF.h:67
DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
Definition: CSerializable.h:174
mrpt::maps::CMultiMetricMapPDF::saveCurrentPathEstimationToTextFile
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...
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: lightweight_geom_data.h:389
mrpt::maps::CMultiMetricMapPDF::getCurrentJointEntropy
double getCurrentJointEntropy()
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Explora...
mrpt::poses::CPose3DPDFParticles
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
Definition: CPose3DPDFParticles.h:37
CParticleFilterCapable.h
mrpt::maps::CMultiMetricMapPDF::insertObservation
void insertObservation(mrpt::obs::CSensoryFrame &sf)
Insert an observation to the map, at each particle's pose and to each particle's metric map.
mrpt::utils::CSerializable
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:40
mrpt::maps::CRBPFParticleData::mapTillNow
CMultiMetricMap mapTillNow
Definition: maps/CMultiMetricMapPDF.h:48
mrpt::maps::CMultiMetricMapPDF::PF_SLAM_implementation_doWeHaveValidObservations
bool PF_SLAM_implementation_doWeHaveValidObservations(const CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const
mrpt::maps::CMultiMetricMapPDF::CMultiMetricMapPDF
CMultiMetricMapPDF(const bayes::CParticleFilter::TParticleFilterOptions &opts=bayes::CParticleFilter::TParticleFilterOptions(), const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers=NULL, const TPredictionParams *predictionOptions=NULL)
Constructor.
DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE
#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...
Definition: CSerializable.h:170
mrpt::maps::CMultiMetricMapPDF::rebuildAverageMap
void rebuildAverageMap()
Rebuild the "expected" grid map.
mrpt::maps::CMultiMetricMapPDF::PF_SLAM_implementation_custom_update_particle_with_new_pose
void PF_SLAM_implementation_custom_update_particle_with_new_pose(CParticleDataContent *particleData, const mrpt::math::TPose3D &newPose) const
mrpt::maps::CMultiMetricMapPDF::TPredictionParams::loadFromConfigFile
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:147
mrpt::maps::CMultiMetricMapPDF::prediction_and_update_pfStandardProposal
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.
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: maps/CSimpleMap.h:35
mrpt::maps::CMultiMetricMapPDF::TPredictionParams
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
Definition: maps/CMultiMetricMapPDF.h:121
mrpt::maps::CMultiMetricMapPDF::TPredictionParams::TPredictionParams
TPredictionParams()
Default settings method.
mrpt::maps::CMultiMetricMapPDF::prediction_and_update_pfOptimalProposal
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.
mrpt::maps::COccupancyGridMap2D::TLikelihoodOptions
With this struct options are provided to the observation likelihood computation process.
Definition: maps/COccupancyGridMap2D.h:392
mrpt::maps::CMultiMetricMapPDF::PF_SLAM_implementation_skipRobotMovement
bool PF_SLAM_implementation_skipRobotMovement() const
mrpt::maps::CRBPFParticleData::CRBPFParticleData
CRBPFParticleData(const TSetOfMetricMapInitializers *mapsInitializers=NULL)
Definition: maps/CMultiMetricMapPDF.h:42
mrpt::maps::CMultiMetricMapPDF::prediction_and_update_pfAuxiliaryPFOptimal
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.
CPosePDFParticles.h
mrpt::bayes::CParticleFilter::TParticleFilterOptions
The configuration of a particle filter.
Definition: CParticleFilter.h:81
mrpt::bayes::CParticleFilterData< CRBPFParticleData >::CParticleDataContent
CRBPFParticleData CParticleDataContent
This is the type inside the corresponding CParticleData class.
Definition: CParticleFilterData.h:180
MRPT_OVERRIDE
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
mrpt::maps::CMultiMetricMapPDF::getNumberOfObservationsInSimplemap
size_t getNumberOfObservationsInSimplemap() const
Get the number of CSensoryFrame inserted into the internal member SFs.
Definition: maps/CMultiMetricMapPDF.h:192
mrpt::maps::CMultiMetricMapPDF::newInfoIndex
float newInfoIndex
An index [0,1] measuring how much information an observation aports to the map (Typ.
Definition: maps/CMultiMetricMapPDF.h:222
mrpt::maps::CMultiMetricMapPDF::getPath
void getPath(size_t i, std::deque< math::TPose3D > &out_path) const
Return the path (in absolute coordinate poses) for the i'th particle.
CPose3DPDFParticles.h
mrpt::maps::CMultiMetricMapPDF::SF2robotPath
std::vector< uint32_t > SF2robotPath
A mapping between indexes in the SFs to indexes in the robot paths from particles.
Definition: maps/CMultiMetricMapPDF.h:107



Page generated by Doxygen 1.8.20 for MRPT 1.4.0 SVN: at Thu Aug 27 02:40:23 UTC 2020