MRPT  1.9.9
CMultiMetricMapPDF.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
12 #include <mrpt/maps/CSimpleMap.h>
15 
17 
20 #include <mrpt/slam/CICP.h>
21 
23 
24 namespace mrpt
25 {
26 namespace slam
27 {
29 }
30 namespace maps
31 {
32 /** Auxiliary class used in mrpt::maps::CMultiMetricMapPDF
33  * \ingroup mrpt_slam_grp
34  */
36 {
38  public:
39  CRBPFParticleData() = default;
41  : mapTillNow(mapsInit), robotPath()
42  {
43  }
44 
46  std::deque<mrpt::math::TPose3D> robotPath;
47 };
48 
49 /** Declares a class that represents a Rao-Blackwellized set of particles for
50  * solving the SLAM problem (This class is the base of RBPF-SLAM applications).
51  * This class is used internally by the map building algorithm in
52  * "mrpt::slam::CMetricMapBuilderRBPF"
53  *
54  * \sa mrpt::slam::CMetricMapBuilderRBPF
55  * \ingroup metric_slam_grp
56  */
59  public mrpt::bayes::CParticleFilterData<CRBPFParticleData>,
61  CMultiMetricMapPDF,
62  mrpt::bayes::CParticleFilterData<CRBPFParticleData>::CParticleList>,
64  CRBPFParticleData, CMultiMetricMapPDF,
65  mrpt::bayes::particle_storage_mode::POINTER>
66 {
68 
70 
71  protected:
72  // PF algorithm implementations:
74  const mrpt::obs::CActionCollection* action,
75  const mrpt::obs::CSensoryFrame* observation,
77  override;
79  const mrpt::obs::CActionCollection* action,
80  const mrpt::obs::CSensoryFrame* observation,
82  override;
84  const mrpt::obs::CActionCollection* action,
85  const mrpt::obs::CSensoryFrame* observation,
87  override;
89  const mrpt::obs::CActionCollection* action,
90  const mrpt::obs::CSensoryFrame* observation,
92  override;
93 
94  private:
95  /** Internal buffer for the averaged map. */
97  bool averageMapIsUpdated{false};
98 
99  /** The SFs and their corresponding pose estimations */
101  /** A mapping between indexes in the SFs to indexes in the robot paths from
102  * particles. */
103  std::vector<uint32_t> SF2robotPath;
104 
105  public:
106  /** The struct for passing extra simulation parameters to the
107  * prediction/update stage
108  * when running a particle filter.
109  * \sa prediction_and_update
110  */
112  {
113  /** Default settings method */
114  TPredictionParams() = default;
115 
116  void loadFromConfigFile(
117  const mrpt::config::CConfigFileBase& source,
118  const std::string& section) override; // See base docs
119  void dumpToTextStream(
120  std::ostream& out) const override; // See base docs
121 
122  /** [pf optimal proposal only] Only for PF algorithm=2 (Exact
123  * "pfOptimalProposal")
124  * Select the map on which to calculate the optimal
125  * proposal distribution. Values:
126  * 0: Gridmap -> Uses Scan matching-based approximation (based on
127  * Stachniss' work)
128  * 1: Landmarks -> Uses matching to approximate optimal
129  * 2: Beacons -> Used for exact optimal proposal in RO-SLAM
130  * 3: Pointsmap -> Uses Scan matching-based approximation with a map
131  * of points (based on Stachniss' work)
132  * Default = 0
133  */
135 
136  /** [prediction stage][pf optimal proposal only] If
137  * useICPGlobalAlign_withGrid=true, this is the minimum quality ratio
138  * [0,1] of the alignment such as it will be accepted. Otherwise, raw
139  * odometry is used for those bad cases (default=0.7).
140  */
142 
144 
145  /** ICP parameters, used only when "PF_algorithm=2" in the particle
146  * filter. */
148 
149  } options;
150 
151  /** Constructor */
152  CMultiMetricMapPDF() = default;
153 
156  const mrpt::maps::TSetOfMetricMapInitializers& mapsInitializers,
157  const TPredictionParams& predictionOptions);
158 
159  /** Clear all elements of the maps, and restore all paths to a single
160  * starting pose */
161  void clear(const mrpt::poses::CPose2D& initialPose);
162  /** \overload */
163  void clear(const mrpt::poses::CPose3D& initialPose);
164  /** Resets the map by loading an already-mapped map for past poses.
165  * Current robot pose should be normally set to the last keyframe
166  * in the simplemap. */
167  void clear(
168  const mrpt::maps::CSimpleMap& prevMap,
169  const mrpt::poses::CPose3D& currentPose);
170 
171  /** Returns the estimate of the robot pose as a particles PDF for the
172  * instant of time "timeStep", from 0 to N-1.
173  * \sa getEstimatedPosePDF
174  */
176  size_t timeStep,
177  mrpt::poses::CPose3DPDFParticles& out_estimation) const;
178 
179  /** Returns the current estimate of the robot pose, as a particles PDF.
180  * \sa getEstimatedPosePDFAtTime
181  */
182  void getEstimatedPosePDF(
183  mrpt::poses::CPose3DPDFParticles& out_estimation) const;
184 
185  /** Returns the weighted averaged map based on the current best estimation.
186  * If you need a persistent copy of this object, please use
187  * "CSerializable::duplicate" and use the copy.
188  * \sa Almost 100% sure you would prefer the best current map, given by
189  * getCurrentMostLikelyMetricMap()
190  */
192 
193  /** Returns a pointer to the current most likely map (associated to the most
194  * likely particle) */
196 
197  /** Get the number of CSensoryFrame inserted into the internal member SFs */
198  size_t getNumberOfObservationsInSimplemap() const { return SFs.size(); }
199  /** Insert an observation to the map, at each particle's pose and to each
200  * particle's metric map.
201  * \param sf The SF to be inserted
202  * \return true if any may was updated, false otherwise
203  */
205 
206  /** Return the path (in absolute coordinate poses) for the i'th particle.
207  * \exception On index out of bounds
208  */
209  void getPath(size_t i, std::deque<math::TPose3D>& out_path) const;
210 
211  /** Returns the current entropy of paths, computed as the average entropy of
212  * poses along the path, where entropy of each pose estimation is computed
213  * as the entropy of the gaussian approximation covariance.
214  */
215  double getCurrentEntropyOfPaths();
216 
217  /** Returns the joint entropy estimation over paths and maps, acording to
218  * "Information Gain-based Exploration Using" by C. Stachniss, G. Grissetti
219  * and W.Burgard.
220  */
221  double getCurrentJointEntropy();
222 
223  /** Update the poses estimation of the member "SFs" according to the current
224  * path belief.
225  */
227 
228  /** A logging utility: saves the current path estimation for each particle
229  * in a text file (a row per particle, each 3-column-entry is a set
230  * [x,y,phi], respectively).
231  */
232  void saveCurrentPathEstimationToTextFile(const std::string& fil);
233 
234  private:
235  /** Rebuild the "expected" grid map. Used internally, do not call */
236  void rebuildAverageMap();
237 
238  /** An index [0,1] measuring how much information an observation aports to
239  * the map (Typ. threshold=0.07) */
240  float newInfoIndex{0};
241 
242  public:
243  /** \name Virtual methods that the PF_implementations assume exist.
244  @{ */
245 
246  // see docs in base
248  const size_t i, bool& pose_is_valid) const override;
249 
251  CParticleDataContent* particleData,
252  const mrpt::math::TPose3D& newPose) const override;
253 
254  // The base implementation is fine for this class.
255  // void PF_SLAM_implementation_replaceByNewParticleSet( ...
256 
258  const CParticleList& particles,
259  const mrpt::obs::CSensoryFrame* sf) const override;
260 
261  bool PF_SLAM_implementation_skipRobotMovement() const override;
262 
263  /** Evaluate the observation likelihood for one particle at a given location
264  */
267  const size_t particleIndexForMap,
268  const mrpt::obs::CSensoryFrame& observation,
269  const mrpt::poses::CPose3D& x) const override;
270  /** @} */
271 
272 }; // End of class def.
273 
274 } // namespace maps
275 } // namespace mrpt
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 override
Evaluate the observation likelihood for one particle at a given location.
void clear(const mrpt::poses::CPose2D &initialPose)
Clear all elements of the maps, and restore all paths to a single starting pose.
mrpt::maps::CMultiMetricMap averageMap
Internal buffer for the averaged map.
void prediction_and_update_pfAuxiliaryPFOptimal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFOptimal" (if no...
The ICP algorithm configuration data.
Definition: CICP.h:69
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
const CMultiMetricMap * getCurrentMostLikelyMetricMap() const
Returns a pointer to the current most likely map (associated to the most likely particle) ...
TPredictionParams()=default
Default settings method.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
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.
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
float ICPGlobalAlign_MinQuality
[prediction stage][pf optimal proposal only] If useICPGlobalAlign_withGrid=true, this is the minimum ...
Option set for KLD algorithm.
Definition: TKLDParams.h:17
void PF_SLAM_implementation_custom_update_particle_with_new_pose(CParticleDataContent *particleData, const mrpt::math::TPose3D &newPose) const override
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Declares a class for storing a collection of robot actions.
void getEstimatedPosePDF(mrpt::poses::CPose3DPDFParticles &out_estimation) const
Returns the current estimate of the robot pose, as a particles PDF.
A set of common data shared by PF implementations for both SLAM and localization. ...
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...
This class allows loading and storing values and vectors of different types from a configuration text...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
CRBPFParticleData(const TSetOfMetricMapInitializers &mapsInit)
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
This template class declares the array of particles and its internal data, managing some memory-relat...
double getCurrentEntropyOfPaths()
Returns the current entropy of paths, computed as the average entropy of poses along the path...
Auxiliary class used in mrpt::maps::CMultiMetricMapPDF.
void prediction_and_update_pfStandardProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
Performs the particle filter prediction/update stages for the algorithm "pfStandardProposal" (if not ...
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"...
void updateSensoryFrameSequence()
Update the poses estimation of the member "SFs" according to the current path belief.
void prediction_and_update_pfOptimalProposal(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
Performs the particle filter prediction/update stages for the algorithm "pfOptimalProposal" (if not i...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::maps::CSimpleMap SFs
The SFs and their corresponding pose estimations.
bool PF_SLAM_implementation_skipRobotMovement() const override
Do not move the particles until the map is populated.
size_t getNumberOfObservationsInSimplemap() const
Get the number of CSensoryFrame inserted into the internal member SFs.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
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:85
mrpt::vision::TStereoCalibResults out
CMultiMetricMapPDF()=default
Constructor.
The configuration of a particle filter.
double getCurrentJointEntropy()
Returns the joint entropy estimation over paths and maps, acording to "Information Gain-based Explora...
bool PF_SLAM_implementation_doWeHaveValidObservations(const CParticleList &particles, const mrpt::obs::CSensoryFrame *sf) const override
std::deque< mrpt::math::TPose3D > robotPath
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
void prediction_and_update_pfAuxiliaryPFStandard(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options) override
Performs the particle filter prediction/update stages for the algorithm "pfAuxiliaryPFStandard" (if n...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
void rebuildAverageMap()
Rebuild the "expected" grid map.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
std::vector< uint32_t > SF2robotPath
A mapping between indexes in the SFs to indexes in the robot paths from particles.
int pfOptimalProposal_mapSelection
[pf optimal proposal only] Only for PF algorithm=2 (Exact "pfOptimalProposal") Select the map on whic...
bool insertObservation(mrpt::obs::CSensoryFrame &sf)
Insert an observation to the map, at each particle&#39;s pose and to each particle&#39;s metric map...
const CMultiMetricMap * getAveragedMetricMapEstimation()
Returns the weighted averaged map based on the current best estimation.
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...
mrpt::maps::CMultiMetricMapPDF::TPredictionParams options
Declares a class that represents a Probability Density function (PDF) of a 3D pose.
mrpt::math::TPose3D getLastPose(const size_t i, bool &pose_is_valid) const override
Return the last robot pose in the i&#39;th particle; is_valid_pose will be false if there is no such last...
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...
CRBPFParticleData CParticleDataContent
This is the type inside the corresponding CParticleData class.
void getPath(size_t i, std::deque< math::TPose3D > &out_path) const
Return the path (in absolute coordinate poses) for the i&#39;th particle.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020