Main MRPT website > C++ reference
MRPT logo
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-2014, 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/slam/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
30 {
32 
33  /** Auxiliary class used in mrpt::slam::CMultiMetricMapPDF
34  * \ingroup mrpt_slam_grp
35  */
37  {
38  // This must be added to any CSerializable derived class:
40  public:
41  CRBPFParticleData( const TSetOfMetricMapInitializers *mapsInitializers = NULL ) :
42  mapTillNow( mapsInitializers ),
43  robotPath()
44  {
45  }
46 
48  std::deque<TPose3D> robotPath;
49  };
50 
51 
53 
54  /** 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).
55  * This class is used internally by the map building algorithm in "mrpt::slam::CMetricMapBuilderRBPF"
56  *
57  * \sa mrpt::slam::CMetricMapBuilderRBPF
58  * \ingroup metric_slam_grp
59  */
61  public mrpt::utils::CSerializable,
65  {
66  friend class CMetricMapBuilderRBPF;
67  //template <class PARTICLE_TYPE, class MYSELF> friend class PF_implementation;
68 
69  // This must be added to any CSerializable derived class:
71 
72  protected:
73  /** The PF algorithm implementation.
74  */
75  void prediction_and_update_pfStandardProposal(
76  const mrpt::slam::CActionCollection * action,
77  const mrpt::slam::CSensoryFrame * observation,
79 
80  /** The PF algorithm implementation.
81  */
82  void prediction_and_update_pfOptimalProposal(
83  const mrpt::slam::CActionCollection * action,
84  const mrpt::slam::CSensoryFrame * observation,
86 
87  /** The PF algorithm implementation.
88  */
89  void prediction_and_update_pfAuxiliaryPFOptimal(
90  const mrpt::slam::CActionCollection * action,
91  const mrpt::slam::CSensoryFrame * observation,
93 
94 
95  private:
96  /** Internal buffer for the averaged map.
97  */
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.
123  */
125 
126  /** See utils::CLoadableOptions
127  */
128  void loadFromConfigFile(
129  const mrpt::utils::CConfigFileBase &source,
130  const std::string &section);
131 
132  /** See utils::CLoadableOptions
133  */
134  void dumpToTextStream(CStream &out) const;
135 
136  /** [pf optimal proposal only] Only for PF algorithm=2 (Exact "pfOptimalProposal")
137  * Select the map on which to calculate the optimal
138  * proposal distribution. Values:
139  * 0: Gridmap -> Uses Scan matching-based approximation (based on Stachniss' work)
140  * 1: Landmarks -> Uses matching to approximate optimal
141  * 2: Beacons -> Used for exact optimal proposal in RO-SLAM
142  * 3: Pointsmap -> Uses Scan matching-based approximation with a map of points (based on Stachniss' work)
143  * Default = 0
144  */
146 
147 
148  /** [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).
149  */
151 
152  /** [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.
153  */
155 
157 
158  CICP::TConfigParams icp_params; //!< ICP parameters, used only when "PF_algorithm=2" in the particle filter.
159 
160  } options;
161 
162  /** Constructor
163  */
166  const mrpt::slam::TSetOfMetricMapInitializers *mapsInitializers = NULL,
167  const TPredictionParams *predictionOptions = NULL );
168 
169  /** Destructor
170  */
171  virtual ~CMultiMetricMapPDF();
172 
173  /** Clear all elements of the maps, and restore all paths to a single starting pose */
174  void clear( const CPose2D &initialPose );
175 
176  /** Clear all elements of the maps, and restore all paths to a single starting pose */
177  void clear( const CPose3D &initialPose );
178 
179  /** Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep", from 0 to N-1.
180  * \sa getEstimatedPosePDF
181  */
182  void getEstimatedPosePDFAtTime(
183  size_t timeStep,
184  CPose3DPDFParticles &out_estimation ) const;
185 
186  /** Returns the current estimate of the robot pose, as a particles PDF.
187  * \sa getEstimatedPosePDFAtTime
188  */
189  void getEstimatedPosePDF( CPose3DPDFParticles &out_estimation ) const;
190 
191  /** 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.
192  */
193  CMultiMetricMap * getCurrentMetricMapEstimation( );
194 
195  /** Returns a pointer to the current most likely map (associated to the most likely particle).
196  */
197  CMultiMetricMap * getCurrentMostLikelyMetricMap( );
198 
199  /** Get the number of CSensoryFrame inserted into the internal member SFs */
200  size_t getNumberOfObservationsInSimplemap() const { return SFs.size(); }
201 
202  /** Insert an observation to the map, at each particle's pose and to each particle's metric map.
203  * \param sf The SF to be inserted
204  */
205  void insertObservation(CSensoryFrame &sf);
206 
207  /** Return the path (in absolute coordinate poses) for the i'th particle.
208  * \exception On index out of bounds
209  */
210  void getPath(size_t i, std::deque<math::TPose3D> &out_path) const;
211 
212  /** 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.
213  */
214  double getCurrentEntropyOfPaths();
215 
216  /** Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Exploration Using" by C. Stachniss, G. Grissetti and W.Burgard.
217  */
218  double getCurrentJointEntropy();
219 
220  /** Update the poses estimation of the member "SFs" according to the current path belief.
221  */
222  void updateSensoryFrameSequence();
223 
224  /** 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).
225  */
226  void saveCurrentPathEstimationToTextFile( const std::string &fil );
227 
228  /** An index [0,1] measuring how much information an observation aports to the map (Typ. threshold=0.07)
229  */
231 
232  private:
233  /** Rebuild the "expected" grid map. Used internally, do not call
234  */
235  void rebuildAverageMap();
236 
237 
238 
239  //protected:
240  public:
241  /** \name Virtual methods that the PF_implementations assume exist.
242  @{ */
243 
244  /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */
245  const TPose3D * getLastPose(const size_t i) const;
246 
247  void PF_SLAM_implementation_custom_update_particle_with_new_pose(
248  CParticleDataContent *particleData,
249  const TPose3D &newPose) const;
250 
251  // The base implementation is fine for this class.
252  // void PF_SLAM_implementation_replaceByNewParticleSet( ...
253 
254  bool PF_SLAM_implementation_doWeHaveValidObservations(
255  const CParticleList &particles,
256  const CSensoryFrame *sf) const;
257 
258  bool PF_SLAM_implementation_skipRobotMovement() const;
259 
260  /** Evaluate the observation likelihood for one particle at a given location */
261  double PF_SLAM_computeObservationLikelihoodForParticle(
262  const CParticleFilter::TParticleFilterOptions &PF_options,
263  const size_t particleIndexForMap,
264  const CSensoryFrame &observation,
265  const CPose3D &x ) const;
266  /** @} */
267 
268 
269  }; // End of class def.
270 
271  } // End of namespace
272 } // End of namespace
273 
274 #endif
The ICP algorithm configuration data.
Definition: CICP.h:57
This class stores any customizable set of metric maps.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
CMultiMetricMap averageMap
Internal buffer for the averaged map.
Option set for KLD algorithm.
Definition: TKLDParams.h:24
size_t size() const
Returns the pairs count.
COccupancyGridMap2D::TLikelihoodOptions update_gridMapLikelihoodOptions
[update stage] If the likelihood is computed through the occupancy grid map, then this structure is p...
std::deque< TPose3D > robotPath
Declares a class for storing a collection of robot actions.
This class allows loading and storing values and vectors of different types from a configuration text...
A set of common data shared by PF implementations for both SLAM and localization. ...
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...
Definition: CStream.h:36
#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::vector< uint32_t > SF2robotPath
A mapping between indexes in the SFs to indexes in the robot paths from particles.
float newInfoIndex
An index [0,1] measuring how much information an observation aports to the map (Typ.
CICP::TConfigParams icp_params
ICP parameters, used only when "PF_algorithm=2" in the particle filter.
size_t getNumberOfObservationsInSimplemap() const
Get the number of CSensoryFrame inserted into the internal member SFs.
This template class declares the array of particles and its internal data, managing some memory-relat...
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:33
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
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 set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
A class used to store a 2D pose.
Definition: CPose2D.h:35
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).
Definition: CPose3D.h:71
The configuration of a particle filter.
Auxiliary class used in mrpt::slam::CMultiMetricMapPDF.
int pfOptimalProposal_mapSelection
[pf optimal proposal only] Only for PF algorithm=2 (Exact "pfOptimalProposal") Select the map on whic...
Declares a class that represents a Rao-Blackwellized set of particles for solving the SLAM problem (T...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
float ICPGlobalAlign_MinQuality
[prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum ...
CRBPFParticleData(const TSetOfMetricMapInitializers *mapsInitializers=NULL)
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
CSimpleMap SFs
The SFs and their corresponding pose estimations:
With this struct options are provided to the observation likelihood computation process.



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo