MRPT  2.0.1
CMetricMapBuilderRBPF.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 
15 #include <mrpt/img/CCanvas.h>
19 
20 namespace mrpt::slam
21 {
22 /** This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to
23  * map building (SLAM).
24  * Internally, the list of particles, each containing a hypothesis for the
25  * robot path plus its associated
26  * metric map, is stored in an object of class CMultiMetricMapPDF.
27  *
28  * This class processes robot actions and observations sequentially (through
29  * the method CMetricMapBuilderRBPF::processActionObservation)
30  * and exploits the generic design of metric map classes in MRPT to deal with
31  * any number and combination of maps simultaneously: the likelihood
32  * of observations is the product of the likelihood in the different maps,
33  * etc.
34  *
35  * A number of particle filter methods are implemented as well, by selecting
36  * the appropriate values in TConstructionOptions::PF_options.
37  * Not all the PF algorithms are implemented for all kinds of maps.
38  *
39  * For an example of usage, check the application "rbpf-slam", in
40  * "apps/RBPF-SLAM". See also the <a
41  * href="http://www.mrpt.org/Application:RBPF-SLAM" >wiki page</a>.
42  *
43  * \note Since MRPT 0.7.2, the new variables
44  * "localizeLinDistance,localizeAngDistance" are introduced to provide a way to
45  * update the robot pose at a different rate than the map is updated.
46  * \note Since MRPT 0.7.1 the semantics of the parameters
47  * "insertionLinDistance" and "insertionAngDistance" changes: the entire RBFP is
48  * now NOT updated unless odometry increments surpass the threshold (previously,
49  * only the map was NOT updated). This is done to gain efficiency.
50  * \note Since MRPT 0.6.2 this class implements full 6D SLAM. Previous versions
51  * worked in 2D + heading only.
52  *
53  * \sa CMetricMap \ingroup metric_slam_grp
54  */
56 {
57  public:
58  /** The map PDF: It includes a path and associated map for each particle. */
60 
61  protected:
62  /** The configuration of the particle filter */
64 
65  /** Distances (linear and angular) for inserting a new observation into the
66  * map. */
68 
69  /** Distances (linear and angular) for updating the robot pose estimate (and
70  * particles weighs, if applicable). */
72 
73  /** Traveled distance since last localization update */
75  /** Traveled distance since last map update */
77 
78  public:
79  /** Options for building a CMetricMapBuilderRBPF object, passed to the
80  * constructor.
81  */
83  {
84  public:
85  /** Constructor */
87  void loadFromConfigFile(
88  const mrpt::config::CConfigFileBase& source,
89  const std::string& section) override; // See base docs
90  void dumpToTextStream(
91  std::ostream& out) const override; // See base docs
92 
93  float insertionLinDistance{1.0f};
95 
96  float localizeLinDistance{0.4f};
98 
100 
104  };
105 
106  /** Constructor. */
107  CMetricMapBuilderRBPF(const TConstructionOptions& initializationOptions);
108 
109  /** This second constructor is created for the situation where a class
110 member needs to be
111 of type CMetricMapBuilderRBPF */
113 
114  /** Copy Operator. */
116 
117  /** Destructor. */
118  ~CMetricMapBuilderRBPF() override;
119 
120  /** Initialize the method, starting with a known location PDF "x0"(if
121  * supplied, set to nullptr to left unmodified) and a given fixed, past map.
122  */
123  void initialize(
124  const mrpt::maps::CSimpleMap& initialMap = mrpt::maps::CSimpleMap(),
125  const mrpt::poses::CPosePDF* x0 = nullptr) override;
126 
127  /** Clear all elements of the maps.
128  */
129  void clear();
130 
131  /** Returns a copy of the current best pose estimation as a pose PDF.
132  */
134 
135  /** Returns the current most-likely path estimation (the path associated to
136  * the most likely particle).
137  */
139  std::deque<mrpt::math::TPose3D>& outPath) const;
140 
141  /** Appends a new action and observations to update this map: See the
142  *description of the class at the top of this page to see a more complete
143  *description.
144  * \param action The incremental 2D pose change in the robot pose. This
145  *value is deterministic.
146  * \param observations The set of observations that robot senses at the new
147  *pose.
148  * Statistics will be saved to statsLastIteration
149  */
152  mrpt::obs::CSensoryFrame& observations) override;
153 
154  /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so
155  * far built map.
156  */
157  void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap& out_map) const override;
158 
159  /** Returns the map built so far. NOTE that for efficiency a pointer to the
160  * internal object is passed, DO NOT delete nor modify the object in any
161  * way, if desired, make a copy of ir with "clone()".
162  */
164  const override;
165 
166  /** Returns just how many sensory-frames are stored in the currently build
167  * map.
168  */
169  unsigned int getCurrentlyBuiltMapSize() override;
170 
171  /** A useful method for debugging: the current map (and/or poses) estimation
172  * is dumped to an image file.
173  * \param file The output file name
174  * \param formatEMF_BMP Output format = true:EMF, false:BMP
175  */
177  const std::string& file, bool formatEMF_BMP = true) override;
178 
179  /** A useful method for debugging: draws the current map and path hypotheses
180  * to a CCanvas */
182 
183  /** A logging utility: saves the current path estimation for each particle
184  * in a text file (a row per particle, each 3-column-entry is a set
185  * [x,y,phi], respectively).
186  */
187  void saveCurrentPathEstimationToTextFile(const std::string& fil);
188 
189  double getCurrentJointEntropy();
190 
191  /** This structure will hold stats after each execution of
192  * processActionObservation
193  */
194  struct TStats
195  {
196  TStats() = default;
197  /** Whether the SF has been inserted in the metric maps. */
198  bool observationsInserted{false};
199  };
200 
201  /** This structure will hold stats after each execution of
202  * processActionObservation */
204 
205 }; // End of class def.
206 
207 } // namespace mrpt::slam
This virtual class defines the interface of any object accepting drawing primitives on it...
Definition: CCanvas.h:41
VerbosityLevel
Enumeration of available verbosity levels.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
bool observationsInserted
Whether the SF has been inserted in the metric maps.
CMetricMapBuilderRBPF & operator=(const CMetricMapBuilderRBPF &src)
Copy Operator.
const mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap() const override
Returns the map built so far.
mrpt::poses::CPose3D odoIncrementSinceLastMapUpdate
Traveled distance since last map update.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
float insertionLinDistance
Distances (linear and angular) for inserting a new observation into the map.
float localizeLinDistance
Distances (linear and angular) for updating the robot pose estimate (and particles weighs...
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...
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true) override
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file...
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), const mrpt::poses::CPosePDF *x0=nullptr) override
Initialize the method, starting with a known location PDF "x0"(if supplied, set to nullptr to left un...
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Declares a class for storing a collection of robot actions.
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &observations) override
Appends a new action and observations to update this map: See the description of the class at the top...
void clear()
Clear all elements of the maps.
This class allows loading and storing values and vectors of different types from a configuration text...
mrpt::maps::CMultiMetricMapPDF mapPDF
The map PDF: It includes a path and associated map for each particle.
~CMetricMapBuilderRBPF() override
Destructor.
CMetricMapBuilderRBPF()
This second constructor is created for the situation where a class member needs to be of type CMetric...
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const override
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
mrpt::poses::CPose3DPDF::Ptr getCurrentPoseEstimation() const override
Returns a copy of the current best pose estimation as a pose PDF.
This virtual class is the base for SLAM implementations.
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
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.
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.
TStats m_statsLastIteration
This structure will hold stats after each execution of processActionObservation.
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
The configuration of a particle filter.
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
void getCurrentMostLikelyPath(std::deque< mrpt::math::TPose3D > &outPath) const
Returns the current most-likely path estimation (the path associated to the most likely particle)...
mrpt::maps::CMultiMetricMapPDF::TPredictionParams predictionOptions
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization
Traveled distance since last localization update.
mrpt::maps::TSetOfMetricMapInitializers mapsInitializers
This structure will hold stats after each execution of processActionObservation.
bayes::CParticleFilter::TParticleFilterOptions PF_options
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...
unsigned int getCurrentlyBuiltMapSize() override
Returns just how many sensory-frames are stored in the currently build map.
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
bayes::CParticleFilter::TParticleFilterOptions m_PF_options
The configuration of the particle filter.
void drawCurrentEstimationToImage(mrpt::img::CCanvas *img)
A useful method for debugging: draws the current map and path hypotheses to a CCanvas.



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020