Main MRPT website > C++ reference for MRPT 1.5.7
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-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 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  public:
67 
68  /** Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
69  */
71  {
72  public:
73  /** Constructor */
75  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
76  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
77 
80 
83 
85 
88  mrpt::utils::VerbosityLevel verbosity_level;
89  };
90 
91  /** Constructor. */
92  CMetricMapBuilderRBPF( const TConstructionOptions &initializationOptions );
93 
94  /** This second constructor is created for the situation where a class member needs to be
95 of type CMetricMapBuilderRBPF */
97 
98  /** Copy Operator. */
99  CMetricMapBuilderRBPF & operator =(const CMetricMapBuilderRBPF &src);
100 
101  /** Destructor. */
102  virtual ~CMetricMapBuilderRBPF( );
103 
104  /** Initialize the method, starting with a known location PDF "x0"
105  * (or set to NULL to use the last keyframe in the simplemap) and a given fixed, past map. */
106  void initialize(
107  const mrpt::maps::CSimpleMap &initialMap = mrpt::maps::CSimpleMap(),
108  mrpt::poses::CPosePDF *x0 = NULL
109  );
110 
111  /** Clear all elements of the maps.
112  */
113  void clear();
114 
115  /** Returns a copy of the current best pose estimation as a pose PDF.
116  */
117  mrpt::poses::CPose3DPDFPtr getCurrentPoseEstimation() const;
118 
119  /** Returns the current most-likely path estimation (the path associated to the most likely particle).
120  */
121  void getCurrentMostLikelyPath( std::deque<mrpt::math::TPose3D> &outPath ) const;
122 
123  /** 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.
124  * \param action The incremental 2D pose change in the robot pose. This value is deterministic.
125  * \param observations The set of observations that robot senses at the new pose.
126  * Statistics will be saved to statsLastIteration
127  */
128  void processActionObservation(
130  mrpt::obs::CSensoryFrame &observations );
131 
132  /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
133  */
134  void getCurrentlyBuiltMap(mrpt::maps::CSimpleMap &out_map) const;
135 
136  /** 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()".
137  */
138  const mrpt::maps::CMultiMetricMap* getCurrentlyBuiltMetricMap() const;
139 
140  /** Returns just how many sensory-frames are stored in the currently build map.
141  */
142  unsigned int getCurrentlyBuiltMapSize();
143 
144  /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
145  * \param file The output file name
146  * \param formatEMF_BMP Output format = true:EMF, false:BMP
147  */
148  void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true);
149 
150  /** A useful method for debugging: draws the current map and path hypotheses to a CCanvas */
151  void drawCurrentEstimationToImage( utils::CCanvas *img );
152 
153  /** 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).
154  */
155  void saveCurrentPathEstimationToTextFile( const std::string &fil );
156 
157  double getCurrentJointEntropy();
158 
159  /** This structure will hold stats after each execution of processActionObservation
160  */
162  {
163  TStats() :
164  observationsInserted(false)
165  { }
166 
167  /** Whether the SF has been inserted in the metric maps. */
169 
170  };
171 
172  /** This structure will hold stats after each execution of processActionObservation */
174 
175  }; // End of class def.
176 
177  } // End of namespace
178 } // End of namespace
179 
180 #endif
bool observationsInserted
Whether the SF has been inserted in the metric maps.
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...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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...
GLuint src
Definition: glext.h:6303
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
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...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
mrpt::maps::CMultiMetricMapPDF mapPDF
The map PDF: It includes a path and associated map for each particle.
GLint GLvoid * img
Definition: glext.h:3645
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
This virtual class is the base for SLAM implementations.
GLsizei const GLchar ** string
Definition: glext.h:3919
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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:72
The configuration of a particle filter.
The struct for passing extra simulation parameters to the prediction/update stage when running a part...
mrpt::maps::CMultiMetricMapPDF::TPredictionParams predictionOptions
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
This virtual class defines the interface of any object accepting drawing primitives on it...
Definition: CCanvas.h:40
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...
Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
bayes::CParticleFilter::TParticleFilterOptions m_PF_options
The configuration of the particle filter.
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019