Main MRPT website > C++ reference for MRPT 1.4.0
CMetricMapBuilderRBPF.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 CMetricMapBuilderRBPF_H
10 #define CMetricMapBuilderRBPF_H
11 
15 
20 
21 #include <mrpt/slam/link_pragmas.h>
22 
23 namespace mrpt
24 {
25 namespace slam
26 {
27  /** This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM).
28  * Internally, the list of particles, each containing a hypothesis for the robot path plus its associated
29  * metric map, is stored in an object of class CMultiMetricMapPDF.
30  *
31  * This class processes robot actions and observations sequentially (through the method CMetricMapBuilderRBPF::processActionObservation)
32  * and exploits the generic design of metric map classes in MRPT to deal with any number and combination of maps simultaneously: the likelihood
33  * of observations is the product of the likelihood in the different maps, etc.
34  *
35  * A number of particle filter methods are implemented as well, by selecting the appropriate values in TConstructionOptions::PF_options.
36  * Not all the PF algorithms are implemented for all kinds of maps.
37  *
38  * For an example of usage, check the application "rbpf-slam", in "apps/RBPF-SLAM". See also the <a href="http://www.mrpt.org/Application:RBPF-SLAM" >wiki page</a>.
39  *
40  * \note Since MRPT 0.7.2, the new variables "localizeLinDistance,localizeAngDistance" are introduced to provide a way to update the robot pose at a different rate than the map is updated.
41  * \note Since MRPT 0.7.1 the semantics of the parameters "insertionLinDistance" and "insertionAngDistance" changes: the entire RBFP is now NOT updated unless odometry increments surpass the threshold (previously, only the map was NOT updated). This is done to gain efficiency.
42  * \note Since MRPT 0.6.2 this class implements full 6D SLAM. Previous versions worked in 2D + heading only.
43  *
44  * \sa CMetricMap \ingroup metric_slam_grp
45  */
47  {
48  public:
49  /** The map PDF: It includes a path and associated map for each particle. */
51 
52  protected:
53  /** The configuration of the particle filter */
55 
56  /** Distances (linear and angular) for inserting a new observation into the map. */
57  float insertionLinDistance,insertionAngDistance;
58 
59  /** Distances (linear and angular) for updating the robot pose estimate (and particles weighs, if applicable). */
60  float localizeLinDistance,localizeAngDistance;
61 
62 
63  mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization; //!< Traveled distance since last localization update
64  mrpt::poses::CPose3D odoIncrementSinceLastMapUpdate; //!< Traveled distance since last map update
65 
66  /** A buffer: memory is actually hold within "mapPDF" */
68 
69  public:
70 
71  /** Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
72  */
74  {
75  public:
76  /** Constructor */
78  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
79  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
80 
83 
86 
88 
91  };
92 
93  /** Constructor. */
94  CMetricMapBuilderRBPF( const TConstructionOptions &initializationOptions );
95 
96  /** This second constructor is created for the situation where a class member needs to be
97 of type CMetricMapBuilderRBPF */
99 
100  /** Copy Operator. */
102 
103  /** Destructor. */
105 
106  /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map. */
108  const mrpt::maps::CSimpleMap &initialMap = mrpt::maps::CSimpleMap(),
109  mrpt::poses::CPosePDF *x0 = NULL
110  );
111 
112  /** Clear all elements of the maps.
113  */
114  void clear();
115 
116  /** Returns a copy of the current best pose estimation as a pose PDF.
117  */
118  mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation() const;
119 
120  /** Returns the current most-likely path estimation (the path associated to the most likely particle).
121  */
122  void getCurrentMostLikelyPath( std::deque<mrpt::math::TPose3D> &outPath ) const;
123 
124  /** Appends a new action and observations to update this map: See the description of the class at the top of this page to see a more complete description.
125  * \param action The incremental 2D pose change in the robot pose. This value is deterministic.
126  * \param observations The set of observations that robot senses at the new pose.
127  * Statistics will be saved to statsLastIteration
128  */
131  mrpt::obs::CSensoryFrame &observations );
132 
133  /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
134  */
136 
137  /** Returns the map built so far. NOTE that for efficiency a pointer to the internal object is passed, DO NOT delete nor modify the object in any way, if desired, make a copy of ir with "duplicate()".
138  */
140 
141  /** Returns just how many sensory-frames are stored in the currently build map.
142  */
143  unsigned int getCurrentlyBuiltMapSize();
144 
145  /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
146  * \param file The output file name
147  * \param formatEMF_BMP Output format = true:EMF, false:BMP
148  */
149  void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true);
150 
151  /** A useful method for debugging: draws the current map and path hypotheses to a CCanvas */
153 
154  /** 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).
155  */
156  void saveCurrentPathEstimationToTextFile( const std::string &fil );
157 
159 
160  /** This structure will hold stats after each execution of processActionObservation
161  */
163  {
164  TStats() :
165  observationsInserted(false)
166  { }
167 
168  /** Whether the SF has been inserted in the metric maps. */
170 
171  };
172 
173  /** This structure will hold stats after each execution of processActionObservation */
175 
176  }; // End of class def.
177 
178  } // End of namespace
179 } // End of namespace
180 
181 #endif
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...
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Declares a class for storing a collection of robot actions.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
Definition: CPosePDF.h:40
This virtual class is the base for SLAM implementations.
This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM).
CMetricMapBuilderRBPF(const TConstructionOptions &initializationOptions)
Constructor.
void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const
Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
void drawCurrentEstimationToImage(utils::CCanvas *img)
A useful method for debugging: draws the current map and path hypotheses to a CCanvas
void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP=true)
A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
void processActionObservation(mrpt::obs::CActionCollection &action, mrpt::obs::CSensoryFrame &observations)
Appends a new action and observations to update this map: See the description of the class at the top...
mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation() const
Returns a copy of the current best pose estimation as a pose PDF.
unsigned int getCurrentlyBuiltMapSize()
Returns just how many sensory-frames are stored in the currently build map.
mrpt::maps::CMultiMetricMapPDF mapPDF
The map PDF: It includes a path and associated map for each particle.
mrpt::utils::non_copiable_ptr< mrpt::maps::CMultiMetricMap > currentMetricMapEstimation
A buffer: memory is actually hold within "mapPDF".
CMetricMapBuilderRBPF()
This second constructor is created for the situation where a class member needs to be of type CMetric...
virtual ~CMetricMapBuilderRBPF()
Destructor.
void initialize(const mrpt::maps::CSimpleMap &initialMap=mrpt::maps::CSimpleMap(), mrpt::poses::CPosePDF *x0=NULL)
Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmod...
TStats m_statsLastIteration
This structure will hold stats after each execution of processActionObservation.
void clear()
Clear all elements of the maps.
bayes::CParticleFilter::TParticleFilterOptions m_PF_options
The configuration of the particle filter.
mrpt::poses::CPose3D odoIncrementSinceLastMapUpdate
Traveled distance since last map update.
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 getCurrentMostLikelyPath(std::deque< mrpt::math::TPose3D > &outPath) const
Returns the current most-likely path estimation (the path associated to the most likely particle).
mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization
Traveled distance since last localization update.
mrpt::maps::CMultiMetricMap * getCurrentlyBuiltMetricMap()
Returns the map built so far.
This virtual class defines the interface of any object accepting drawing primitives on it.
Definition: CCanvas.h:41
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The configuration of a particle filter.
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
bayes::CParticleFilter::TParticleFilterOptions PF_options
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
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.
mrpt::maps::CMultiMetricMapPDF::TPredictionParams predictionOptions
mrpt::maps::TSetOfMetricMapInitializers mapsInitializers
This structure will hold stats after each execution of processActionObservation.
bool observationsInserted
Whether the SF has been inserted in the metric maps.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 04:07:33 UTC 2022