Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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  */
62  class SLAM_IMPEXP CMultiMetricMapPDF :
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:
71  DEFINE_SERIALIZABLE( CMultiMetricMapPDF )
72 
73  protected:
74  // PF algorithm implementations:
75  void prediction_and_update_pfStandardProposal(
76  const mrpt::obs::CActionCollection * action,
77  const mrpt::obs::CSensoryFrame * observation,
78  const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) MRPT_OVERRIDE;
79  void prediction_and_update_pfOptimalProposal(
80  const mrpt::obs::CActionCollection * action,
81  const mrpt::obs::CSensoryFrame * observation,
82  const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) MRPT_OVERRIDE;
83  void prediction_and_update_pfAuxiliaryPFOptimal(
84  const mrpt::obs::CActionCollection * action,
85  const mrpt::obs::CSensoryFrame * observation,
86  const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) MRPT_OVERRIDE;
87  void prediction_and_update_pfAuxiliaryPFStandard(
88  const mrpt::obs::CActionCollection * action,
89  const mrpt::obs::CSensoryFrame * observation,
90  const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) MRPT_OVERRIDE;
91 
92 
93  private:
94  /** Internal buffer for the averaged map. */
95  mrpt::maps::CMultiMetricMap averageMap;
96  bool averageMapIsUpdated;
97 
98  mrpt::maps::CSimpleMap SFs; //!< The SFs and their corresponding pose estimations
99  std::vector<uint32_t> SF2robotPath; //!< A mapping between indexes in the SFs to indexes in the robot paths from particles.
100 
101  public:
102 
103  /** The struct for passing extra simulation parameters to the prediction/update stage
104  * when running a particle filter.
105  * \sa prediction_and_update
106  */
107  struct SLAM_IMPEXP TPredictionParams : public utils::CLoadableOptions
108  {
109  /** Default settings method */
111 
112  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
113  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
114 
115  /** [pf optimal proposal only] Only for PF algorithm=2 (Exact "pfOptimalProposal")
116  * Select the map on which to calculate the optimal
117  * proposal distribution. Values:
118  * 0: Gridmap -> Uses Scan matching-based approximation (based on Stachniss' work)
119  * 1: Landmarks -> Uses matching to approximate optimal
120  * 2: Beacons -> Used for exact optimal proposal in RO-SLAM
121  * 3: Pointsmap -> Uses Scan matching-based approximation with a map of points (based on Stachniss' work)
122  * Default = 0
123  */
125 
126 
127  /** [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).
128  */
130 
131  /** [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.
132  */
134 
136 
137  mrpt::slam::CICP::TConfigParams icp_params; //!< ICP parameters, used only when "PF_algorithm=2" in the particle filter.
138 
139  } options;
140 
141  /** Constructor
142  */
145  const mrpt::maps::TSetOfMetricMapInitializers *mapsInitializers = NULL,
146  const TPredictionParams *predictionOptions = NULL );
147 
148  /** Clear all elements of the maps, and restore all paths to a single starting pose */
149  void clear( const mrpt::poses::CPose2D &initialPose );
150  void clear( const mrpt::poses::CPose3D &initialPose ); //!< \overload
151  /** Resets the map by loading an already-mapped map for past poses.
152  * Current robot pose should be normally set to the last keyframe in the simplemap. */
153  void clear(const mrpt::maps::CSimpleMap &prevMap, const mrpt::poses::CPose3D &currentPose);
154 
155  /** Returns the estimate of the robot pose as a particles PDF for the instant of time "timeStep", from 0 to N-1.
156  * \sa getEstimatedPosePDF
157  */
158  void getEstimatedPosePDFAtTime(
159  size_t timeStep,
160  mrpt::poses::CPose3DPDFParticles &out_estimation ) const;
161 
162  /** Returns the current estimate of the robot pose, as a particles PDF.
163  * \sa getEstimatedPosePDFAtTime
164  */
165  void getEstimatedPosePDF( mrpt::poses::CPose3DPDFParticles &out_estimation ) const;
166 
167  /** 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.
168  * \sa Almost 100% sure you would prefer the best current map, given by getCurrentMostLikelyMetricMap()
169  */
170  const CMultiMetricMap * getAveragedMetricMapEstimation();
171 
172  /** Returns a pointer to the current most likely map (associated to the most likely particle) */
173  const CMultiMetricMap * getCurrentMostLikelyMetricMap() const;
174 
175  /** Get the number of CSensoryFrame inserted into the internal member SFs */
176  size_t getNumberOfObservationsInSimplemap() const { return SFs.size(); }
177 
178  /** Insert an observation to the map, at each particle's pose and to each particle's metric map.
179  * \param sf The SF to be inserted
180  * \return true if any may was updated, false otherwise
181  */
182  bool insertObservation(mrpt::obs::CSensoryFrame &sf);
183 
184  /** Return the path (in absolute coordinate poses) for the i'th particle.
185  * \exception On index out of bounds
186  */
187  void getPath(size_t i, std::deque<math::TPose3D> &out_path) const;
188 
189  /** 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.
190  */
191  double getCurrentEntropyOfPaths();
192 
193  /** Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Exploration Using" by C. Stachniss, G. Grissetti and W.Burgard.
194  */
195  double getCurrentJointEntropy();
196 
197  /** Update the poses estimation of the member "SFs" according to the current path belief.
198  */
199  void updateSensoryFrameSequence();
200 
201  /** 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).
202  */
204 
205  private:
206  /** Rebuild the "expected" grid map. Used internally, do not call */
207  void rebuildAverageMap();
208 
209  float newInfoIndex; //!< An index [0,1] measuring how much information an observation aports to the map (Typ. threshold=0.07)
210 
211  public:
212  /** \name Virtual methods that the PF_implementations assume exist.
213  @{ */
214 
215  mrpt::math::TPose3D getLastPose(const size_t i, bool &pose_is_valid) const MRPT_OVERRIDE; // see docs in base
216 
217  void PF_SLAM_implementation_custom_update_particle_with_new_pose(
218  CParticleDataContent *particleData,
219  const mrpt::math::TPose3D &newPose) const MRPT_OVERRIDE;
220 
221  // The base implementation is fine for this class.
222  // void PF_SLAM_implementation_replaceByNewParticleSet( ...
223 
224  bool PF_SLAM_implementation_doWeHaveValidObservations(
225  const CParticleList &particles,
226  const mrpt::obs::CSensoryFrame *sf) const MRPT_OVERRIDE;
227 
228  bool PF_SLAM_implementation_skipRobotMovement() const MRPT_OVERRIDE;
229 
230  /** Evaluate the observation likelihood for one particle at a given location */
231  double PF_SLAM_computeObservationLikelihoodForParticle(
232  const mrpt::bayes::CParticleFilter::TParticleFilterOptions &PF_options,
233  const size_t particleIndexForMap,
234  const mrpt::obs::CSensoryFrame &observation,
235  const mrpt::poses::CPose3D &x ) const MRPT_OVERRIDE;
236  /** @} */
237 
238 
239  }; // End of class def.
240  DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE( CMultiMetricMapPDF, mrpt::utils::CSerializable, SLAM_IMPEXP )
241 
242  } // End of namespace
243 } // End of namespace
244 
245 #endif
The ICP algorithm configuration data.
Definition: CICP.h:57
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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. threshold=0.07)
float ICPGlobalAlign_MinQuality
[prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum ...
mrpt::maps::CMultiMetricMapPDF CMultiMetricMapPDF
Backward compatible typedef.
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...
Option set for KLD algorithm.
Definition: TKLDParams.h:22
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
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...
Definition: CStream.h:38
#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.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
size_t getNumberOfObservationsInSimplemap() const
Get the number of CSensoryFrame inserted into the internal member SFs.
#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.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
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:72
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.
unsigned __int32 uint32_t
Definition: rptypes.h:49
#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...
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
Declares a class that represents a Probability Density function (PDF) of a 3D pose.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018