Main MRPT website > C++ reference
MRPT logo
CMetricMapBuilderRBPF.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CMetricMapBuilderRBPF_H
29 #define CMetricMapBuilderRBPF_H
30 
31 #include <mrpt/slam/CMetricMapBuilder.h>
32 #include <mrpt/slam/CMultiMetricMapPDF.h>
33 #include <mrpt/slam/CMultiMetricMap.h>
34 
35 #include <mrpt/bayes/CParticleFilter.h>
36 #include <mrpt/bayes/CParticleFilterCapable.h>
37 #include <mrpt/utils/CLoadableOptions.h>
38 #include <mrpt/utils/safe_pointers.h>
39 
40 #include <mrpt/slam/link_pragmas.h>
41 
42 namespace mrpt
43 {
44 namespace slam
45 {
46  /** This class implements a Rao-Blackwelized Particle Filter (RBPF) approach to map building (SLAM).
47  * Internally, the list of particles, each containing a hypothesis for the robot path plus its associated
48  * metric map, is stored in an object of class CMultiMetricMapPDF.
49  *
50  * This class processes robot actions and observations sequentially (through the method CMetricMapBuilderRBPF::processActionObservation)
51  * and exploits the generic design of metric map classes in MRPT to deal with any number and combination of maps simultaneously: the likelihood
52  * of observations is the product of the likelihood in the different maps, etc.
53  *
54  * A number of particle filter methods are implemented as well, by selecting the appropriate values in TConstructionOptions::PF_options.
55  * Not all the PF algorithms are implemented for all kinds of maps.
56  *
57  * 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>.
58  *
59  * \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.
60  * \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.
61  * \note Since MRPT 0.6.2 this class implements full 6D SLAM. Previous versions worked in 2D + heading only.
62  *
63  * \sa CMetricMap \ingroup metric_slam_grp
64  */
65  class SLAM_IMPEXP CMetricMapBuilderRBPF : public CMetricMapBuilder
66  {
67  public:
68  /** The map PDF: It includes a path and associated map for each particle.
69  */
70  CMultiMetricMapPDF mapPDF;
71 
72  protected:
73  /** The configuration of the particle filter:
74  */
75  bayes::CParticleFilter::TParticleFilterOptions m_PF_options;
76 
77  /** Distances (linear and angular) for inserting a new observation into the map.
78  */
79  float insertionLinDistance,insertionAngDistance;
80 
81  /** Distances (linear and angular) for updating the robot pose estimate (and particles weighs, if applicable).
82  */
83  float localizeLinDistance,localizeAngDistance;
84 
85 
86  mrpt::poses::CPose3DPDFGaussian odoIncrementSinceLastLocalization; //!< Traveled distance since last localization update
87  mrpt::poses::CPose3D odoIncrementSinceLastMapUpdate; //!< Traveled distance since last map update
88 
89  /** A buffer: memory is actually hold within "mapPDF".
90  */
91  non_copiable_ptr<CMultiMetricMap> currentMetricMapEstimation;
92 
93  public:
94 
95  /** Options for building a CMetricMapBuilderRBPF object, passed to the constructor.
96  */
97  struct SLAM_IMPEXP TConstructionOptions : public utils::CLoadableOptions
98  {
99  public:
100  /** Constructor
101  */
102  TConstructionOptions();
103 
104  /** See utils::CLoadableOptions
105  */
106  void loadFromConfigFile(
107  const mrpt::utils::CConfigFileBase &source,
108  const std::string &section);
109 
110  /** See utils::CLoadableOptions
111  */
112  void dumpToTextStream(CStream &out) const;
113 
114  float insertionLinDistance;
115  float insertionAngDistance;
116 
117  float localizeLinDistance;
118  float localizeAngDistance;
119 
120  bayes::CParticleFilter::TParticleFilterOptions PF_options;
121 
122  TSetOfMetricMapInitializers mapsInitializers;
123  CMultiMetricMapPDF::TPredictionParams predictionOptions;
124  };
125 
126  /** Constructor.
127  */
128  CMetricMapBuilderRBPF( const TConstructionOptions &initializationOptions );
129 
130  /** Destructor.
131  */
132  virtual ~CMetricMapBuilderRBPF( );
133 
134  /** Initialize the method, starting with a known location PDF "x0"(if supplied, set to NULL to left unmodified) and a given fixed, past map.
135  */
136  void initialize(
137  const CSimpleMap &initialMap = CSimpleMap(),
138  CPosePDF *x0 = NULL
139  );
140 
141  /** Clear all elements of the maps.
142  */
143  void clear();
144 
145  /** Returns a copy of the current best pose estimation as a pose PDF.
146  */
147  CPose3DPDFPtr getCurrentPoseEstimation() const;
148 
149  /** Returns the current most-likely path estimation (the path associated to the most likely particle).
150  */
151  void getCurrentMostLikelyPath( std::deque<TPose3D> &outPath ) const;
152 
153  /** 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.
154  * \param action The incremental 2D pose change in the robot pose. This value is deterministic.
155  * \param observations The set of observations that robot senses at the new pose.
156  * Statistics will be saved to statsLastIteration
157  */
158  void processActionObservation(
159  CActionCollection &action,
160  CSensoryFrame &observations );
161 
162  /** Fills "out_map" with the set of "poses"-"sensory-frames", thus the so far built map.
163  */
164  void getCurrentlyBuiltMap(CSimpleMap &out_map) const;
165 
166  /** 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()".
167  */
168  CMultiMetricMap* getCurrentlyBuiltMetricMap();
169 
170  /** Returns just how many sensory-frames are stored in the currently build map.
171  */
172  unsigned int getCurrentlyBuiltMapSize();
173 
174  /** A useful method for debugging: the current map (and/or poses) estimation is dumped to an image file.
175  * \param file The output file name
176  * \param formatEMF_BMP Output format = true:EMF, false:BMP
177  */
178  void saveCurrentEstimationToImage(const std::string &file, bool formatEMF_BMP = true);
179 
180  /** A usefull method for debugging: draws the current map and path hypotheses to a CCanvas
181  */
182  void drawCurrentEstimationToImage( utils::CCanvas *img );
183 
184  /** 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).
185  */
186  void saveCurrentPathEstimationToTextFile( std::string fil );
187 
188  double getCurrentJointEntropy();
189 
190  /** This structure will hold stats after each execution of processActionObservation
191  */
192  struct SLAM_IMPEXP TStats
193  {
194  TStats() :
195  observationsInserted(false)
196  { }
197 
198  /** Whether the SF has been inserted in the metric maps. */
199  bool observationsInserted;
200 
201  };
202 
203 
204  /** This structure will hold stats after each execution of processActionObservation
205  */
206  TStats m_statsLastIteration;
207 
208  }; // End of class def.
209 
210  } // End of namespace
211 } // End of namespace
212 
213 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013