Main MRPT website > C++ reference
MRPT logo
CMultiMetricMap.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-2014, 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 CMultiMetricMap_H
10 #define CMultiMetricMap_H
11 
13 #include <mrpt/slam/COctoMap.h>
23 #include <mrpt/slam/CBeaconMap.h>
24 #include <mrpt/slam/CMetricMap.h>
27 #include <mrpt/utils/TEnumType.h>
28 
29 #include <mrpt/slam/link_pragmas.h>
30 
31 namespace mrpt
32 {
33 namespace slam
34 {
35  class TSetOfMetricMapInitializers;
36 
37  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CMultiMetricMap , CMetricMap, SLAM_IMPEXP )
38 
39  /** This class stores any customizable set of metric maps.
40  * The internal metric maps can be accessed directly by the user as smart pointers.
41  * The intended utility of this container is to operate on several maps simultaneously: update them by inserting observations,
42  * evaluate the likelihood of one observation by fusing (multiplying) the likelihoods over the different maps, etc.
43  *
44  * <b>All these kinds of metric maps can be kept in a multi-metric map:</b>:
45  * - mrpt::slam::CPointsMap: For laser 2D range scans, and posibly for IR ranges,... (It keeps the full 3D structure of scans)
46  * - mrpt::slam::COccupancyGridMap2D: Exclusively for 2D, <b>horizontal</b> laser range scans, at different altitudes.
47  * - mrpt::slam::COctoMap: For 3D occupancy grids of variable resolution, with octrees (based on the library "octomap").
48  * - mrpt::slam::CColouredOctoMap: The same than above, but nodes can store RGB data appart from occupancy.
49  * - mrpt::slam::CLandmarksMap: For visual landmarks,etc...
50  * - mrpt::slam::CGasConcentrationGridMap2D: For gas concentration maps.
51  * - mrpt::slam::CWirelessPowerGridMap2D: For wifi power maps.
52  * - mrpt::slam::CBeaconMap: For range-only SLAM.
53  * - mrpt::slam::CHeightGridMap2D: For maps of height for each (x,y) location.
54  * - mrpt::slam::CReflectivityGridMap2D: For maps of "reflectivity" for each (x,y) location.
55  * - mrpt::slam::CColouredPointsMap: For point map with color.
56  * - mrpt::slam::CWeightedPointsMap: For point map with weights (capable of "fusing").
57  *
58  * See CMultiMetricMap::setListOfMaps() for the method for initializing this class programatically.
59  * See also TSetOfMetricMapInitializers::loadFromConfigFile for a template of ".ini"-like configuration
60  * file that can be used to define which maps to create and all their parameters.
61  *
62  * \sa CMetricMap \ingroup mrpt_slam_grp
63  */
65  {
66  // This must be added to any CSerializable derived class:
68 
69  protected:
70  /** Deletes all maps and clears the internal lists of maps.
71  */
72  void deleteAllMaps();
73 
74  /** Clear all elements of the map.
75  */
76  virtual void internal_clear();
77 
78  /** Insert the observation information into this map (see options)
79  * \param obs The observation
80  * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg)
81  *
82  * \sa CObservation::insertObservationInto
83  */
84  virtual bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
85 
86  public:
87  typedef std::pair<CPoint3D,unsigned int> TPairIdBeacon;
88 
89  /** Returns true if the map is empty/no observation has been inserted.
90  */
91  bool isEmpty() const;
92 
93  /** Some options for this class:
94  */
95  struct SLAM_IMPEXP TOptions : public utils::CLoadableOptions
96  {
97  TOptions();
98 
99  /** Load parameters from configuration source
100  */
101  void loadFromConfigFile(
102  const mrpt::utils::CConfigFileBase &source,
103  const std::string &section);
104 
105  /** This method must display clearly all the contents of the structure in textual form, sending it to a CStream.
106  */
107  void dumpToTextStream(CStream &out) const;
108 
109  /** This selects the map to be used when computing the likelihood of an observation.
110  * This enum has a corresponding mrpt::utils::TEnumType<> specialization.
111  * \sa computeObservationLikelihood
112  */
114  {
115  mapFuseAll = -1,
116  mapGrid = 0,
127  mapColourOctoMaps
128  } likelihoodMapSelection;
129 
130  bool enableInsertion_pointsMap; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
131  bool enableInsertion_landmarksMap; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
132  bool enableInsertion_gridMaps; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
133  bool enableInsertion_gasGridMaps; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
134  bool enableInsertion_wifiGridMaps; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
135  bool enableInsertion_beaconMap; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
136  bool enableInsertion_heightMaps; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
137  bool enableInsertion_reflectivityMaps; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
138  bool enableInsertion_colourPointsMaps; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
139  bool enableInsertion_weightedPointsMaps; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
140  bool enableInsertion_octoMaps; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
141  bool enableInsertion_colourOctoMaps; //!< Default = true (set to false to avoid "insertObservation" to update a given map)
142 
143  } options;
144 
145 
146  /** @name Internal lists of maps
147  @{ */
148  // Note: A variable number of maps may exist, depending on the initialization from TSetOfMetricMapInitializers.
149  // Not used maps are "NULL" or empty smart pointers.
150 
151  std::deque<CSimplePointsMapPtr> m_pointsMaps;
152  std::deque<COccupancyGridMap2DPtr> m_gridMaps;
153  std::deque<COctoMapPtr> m_octoMaps;
154  std::deque<CColouredOctoMapPtr> m_colourOctoMaps;
155  std::deque<CGasConcentrationGridMap2DPtr> m_gasGridMaps;
156  std::deque<CWirelessPowerGridMap2DPtr> m_wifiGridMaps;
157  std::deque<CHeightGridMap2DPtr> m_heightMaps;
158  std::deque<CReflectivityGridMap2DPtr> m_reflectivityMaps;
159  CColouredPointsMapPtr m_colourPointsMap;
160  CWeightedPointsMapPtr m_weightedPointsMap;
161  CLandmarksMapPtr m_landmarksMap;
162  CBeaconMapPtr m_beaconMap;
163 
164  /** @} */
165 
166  /** Constructor.
167  * \param initializers One internal map will be created for each entry in this "TSetOfMetricMapInitializers" struct, and each map will be initialized with the corresponding options.
168  * \param opts If provided (not NULL), the member "options" will be initialized with those values.
169  * If initializers is NULL, no internal map will be created.
170  */
172  const mrpt::slam::TSetOfMetricMapInitializers *initializers = NULL,
173  const TOptions *opts = NULL );
174 
175  /** Sets the list of internal map according to the passed list of map initializers (Current maps' content will be deleted!)
176  */
177  void setListOfMaps( const mrpt::slam::TSetOfMetricMapInitializers *initializers );
178 
179  /** Copy constructor */
181 
182  /** Copy operator from "other" object.
183  */
184  mrpt::slam::CMultiMetricMap &operator = ( const mrpt::slam::CMultiMetricMap &other );
185 
186  /** Destructor.
187  */
188  virtual ~CMultiMetricMap( );
189 
190 
191  // See docs in base class
192  double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
193 
194  /** Returns the ratio of points in a map which are new to the point map while falling into yet static cells of gridmap.
195  * \param points The set of points to check.
196  * \param takenFrom The pose for the reference system of points, in global coordinates of this hybrid map.
197  */
198  float getNewStaticPointsRatio(
199  CPointsMap *points,
200  CPose2D &takenFrom );
201 
202  // See docs in base class.
203  virtual void determineMatching2D(
204  const CMetricMap * otherMap,
205  const CPose2D & otherMapPose,
206  TMatchingPairList & correspondences,
207  const TMatchingParams & params,
208  TMatchingExtraResults & extraResults ) const;
209 
210  /** See the definition in the base class: Calls in this class become a call to every single map in this set. */
211  float compute3DMatchingRatio(
212  const CMetricMap *otherMap,
213  const CPose3D &otherMapPose,
214  float maxDistForCorr = 0.10f,
215  float maxMahaDistForCorr = 2.0f
216  ) const;
217 
218  /** The implementation in this class just calls all the corresponding method of the contained metric maps.
219  */
220  void saveMetricMapRepresentationToFile(
221  const std::string &filNamePrefix
222  ) const;
223 
224  /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
225  * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
226  */
227  void auxParticleFilterCleanUp();
228 
229  /** Returns a 3D object representing the map.
230  */
231  void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
232 
233  /** Returns true if any of the inner maps is able to compute a sensible likelihood function for this observation.
234  * \param obs The observation.
235  * \sa computeObservationLikelihood
236  */
237  bool canComputeObservationLikelihood( const CObservation *obs );
238 
239  /** If the map is a simple point map or it's a multi-metric map that contains EXACTLY one simple point map, return it.
240  * Otherwise, return NULL
241  */
242  virtual const CSimplePointsMap * getAsSimplePointsMap() const;
243  virtual CSimplePointsMap * getAsSimplePointsMap();
244 
245  /** An auxiliary variable that can be used freely by the users (this will be copied to other maps using the copy constructor, copy operator, streaming,etc) The default value is 0.
246  */
247  unsigned int m_ID;
248 
249  }; // End of class def.
251 
252  /** Each structure of this kind will determine the kind (and initial configuration) of one map to be build into a CMultiMetricMap object.
253  * See "mrpt::slam::TSetOfMetricMapInitializers::loadFromConfigFile" as an easy way of initialize this object.
254  * \sa TSetOfMetricMapInitializers, CMultiMetricMap::CMultiMetricMap
255  */
257  {
258  /** Initialization (sets 'metricMapClassType' to NULL, an invalid value -> it must be set correctly before use!)
259  */
260  TMetricMapInitializer();
261 
262  /** Set this to CLASS_ID(< class >) where < class > is any CMetricMap derived class.
263  */
265 
266  /** This value will be copied to the member with the same value in the map, see mrpt::slam::CMetricMap::m_disableSaveAs3DObject
267  */
269 
270  /** Specific options for 2D grid maps (mrpt::slam::COccupancyGridMap2D)
271  */
273  {
274  TOccGridMap2DOptions(); //!< Default values loader
275 
276  float min_x,max_x,min_y,max_y,resolution; //!< See COccupancyGridMap2D::COccupancyGridMap2D
277  COccupancyGridMap2D::TInsertionOptions insertionOpts; //!< Customizable initial options.
278  COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts; //!< Customizable initial options.
279 
280  } occupancyGridMap2D_options;
281 
282  /** Specific options for 3D octo maps (mrpt::slam::COctoMap) */
284  {
285  TOctoMapOptions(); //!< Default values loader
286 
287  double resolution; //!< The finest resolution of the octomap (default: 0.10 meters)
288  COctoMap::TInsertionOptions insertionOpts; //!< Customizable initial options.
289  COctoMap::TLikelihoodOptions likelihoodOpts; //!< Customizable initial options.
290  } octoMap_options;
291 
292  /** Specific options for 3D octo maps (mrpt::slam::COctoMap) */
294  {
295  TColourOctoMapOptions(); //!< Default values loader
296 
297  double resolution; //!< The finest resolution of the octomap (default: 0.10 meters)
298  CColouredOctoMap::TInsertionOptions insertionOpts; //!< Customizable initial options.
299  CColouredOctoMap::TLikelihoodOptions likelihoodOpts; //!< Customizable initial options.
300  } colourOctoMap_options;
301 
302  /** Specific options for point maps (mrpt::slam::CPointsMap)
303  */
305  {
306  CPointsMapOptions(); //!< Default values loader
307  CPointsMap::TInsertionOptions insertionOpts; //!< Customizable initial options for loading the class' own defaults.
308  CPointsMap::TLikelihoodOptions likelihoodOpts; //!< //!< Customizable initial likelihood options
309  } pointsMapOptions_options;
310 
311  /** Specific options for gas grid maps (mrpt::slam::CGasConcentrationGridMap2D)
312  */
314  {
315  CGasConcentrationGridMap2DOptions(); //!< Default values loader
316 
317  float min_x,max_x,min_y,max_y,resolution; //!< See CGasConcentrationGridMap2D::CGasConcentrationGridMap2D
318  CGasConcentrationGridMap2D::TMapRepresentation mapType; //!< The kind of map representation (see CGasConcentrationGridMap2D::CGasConcentrationGridMap2D)
320 
321  } gasGridMap_options;
322 
323  /** Specific options for wifi grid maps (mrpt::slam::CWirelessPowerGridMap2D)
324  */
326  {
327  CWirelessPowerGridMap2DOptions(); //!< Default values loader
328 
329  float min_x,max_x,min_y,max_y,resolution; //!< See CWirelessPowerGridMap2D::CWirelessPowerGridMap2D
330  CWirelessPowerGridMap2D::TMapRepresentation mapType; //!< The kind of map representation (see CWirelessPowerGridMap2D::CWirelessPowerGridMap2D)
331  CWirelessPowerGridMap2D::TInsertionOptions insertionOpts; //!< Customizable initial options.
332 
333  } wifiGridMap_options;
334 
335  /** Specific options for landmarks maps (mrpt::slam::CLandmarksMap)
336  */
338  {
339  CLandmarksMapOptions(); //!< Default values loader
340 
341  std::deque<CMultiMetricMap::TPairIdBeacon> initialBeacons; //!< Initial contents of the map, especified by a set of 3D Beacons with associated IDs
342  CLandmarksMap::TInsertionOptions insertionOpts; //!< Customizable initial options.
343  CLandmarksMap::TLikelihoodOptions likelihoodOpts; //!< Customizable initial options.
344 
345  } landmarksMap_options;
346 
347 
348  /** Specific options for landmarks maps (mrpt::slam::CBeaconMap)
349  */
351  {
352  CBeaconMapOptions(); //!< Default values loader
353 
354  CBeaconMap::TLikelihoodOptions likelihoodOpts; //!< Customizable initial options.
355  CBeaconMap::TInsertionOptions insertionOpts; //!< Customizable initial options.
356 
357  } beaconMap_options;
358 
359  /** Specific options for height grid maps (mrpt::slam::CHeightGridMap2D)
360  */
362  {
363  CHeightGridMap2DOptions(); //!< Default values loader
364 
365  float min_x,max_x,min_y,max_y,resolution; //!< See CHeightGridMap2D::CHeightGridMap2D
366  CHeightGridMap2D::TMapRepresentation mapType; //!< The kind of map representation (see CHeightGridMap2D::CHeightGridMap2D)
367  CHeightGridMap2D::TInsertionOptions insertionOpts; //!< Customizable initial options.
368  } heightMap_options;
369 
370  /** Specific options for height grid maps (mrpt::slam::CReflectivityGridMap2D)
371  */
373  {
374  CReflectivityGridMap2DOptions(); //!< Default values loader
375 
376  float min_x,max_x,min_y,max_y,resolution; //!< See CReflectivityGridMap2DOptions::CReflectivityGridMap2DOptions
377  CReflectivityGridMap2D::TInsertionOptions insertionOpts; //!< Customizable initial options.
378  } reflectivityMap_options;
379 
380  /** Specific options for coloured point maps (mrpt::slam::CPointsMap)
381  */
383  {
384  CColouredPointsMapOptions(); //!< Default values loader
385  CPointsMap::TInsertionOptions insertionOpts; //!< Customizable initial options for loading the class' own defaults.
386  CPointsMap::TLikelihoodOptions likelihoodOpts; //!< //!< Customizable initial likelihood options
387  CColouredPointsMap::TColourOptions colourOpts; //!< Customizable initial options for loading the class' own defaults. */
388  } colouredPointsMapOptions_options;
389 
390  /** Specific options for coloured point maps (mrpt::slam::CPointsMap)
391  */
393  {
394  CWeightedPointsMapOptions(); //!< Default values loader
395  CPointsMap::TInsertionOptions insertionOpts; //!< Customizable initial options for loading the class' own defaults.
396  CPointsMap::TLikelihoodOptions likelihoodOpts; //!< //!< Customizable initial likelihood options
397  } weightedPointsMapOptions_options;
398  };
399 
400  /** A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap
401  * See the comments for TSetOfMetricMapInitializers::loadFromConfigFile, and "CMultiMetricMap::setListOfMaps" for
402  * effectively creating the list of desired maps.
403  * \sa CMultiMetricMap::CMultiMetricMap, utils::CLoadableOptions
404  */
406  {
407  protected:
408  std::deque<TMetricMapInitializer> m_list;
409 
410  public:
411  size_t size() const { return m_list.size(); }
412  void push_back( const TMetricMapInitializer &o ) { m_list.push_back(o); }
413 
416 
417  iterator begin() { return m_list.begin(); }
418  const_iterator begin() const { return m_list.begin(); }
419 
420  iterator end() { return m_list.end(); }
421  const_iterator end() const { return m_list.end(); }
422 
423  void clear() { m_list.clear(); }
424 
425 
426  TSetOfMetricMapInitializers() : m_list(), options()
427  {}
428 
429 
430  /** This options will be loaded when creating the set of maps in CMultiMetricMap (See CMultiMetricMap::TOptions)
431  */
433 
434  /** Loads the configuration for the set of internal maps from a textual definition in an INI-like file.
435  * The format of the ini file is defined in utils::CConfigFile. The list of maps and their options
436  * will be loaded from a handle of sections:
437  *
438  * \code
439  * [<sectionName>]
440  * // Creation of maps:
441  * occupancyGrid_count=<Number of mrpt::slam::COccupancyGridMap2D maps>
442  * octoMap_count=<Number of mrpt::slam::COctoMap maps>
443  * colourOctoMap_count=<Number of mrpt::slam::CColourOctoMap maps>
444  * gasGrid_count=<Number of mrpt::slam::CGasConcentrationGridMap2D maps>
445  * wifiGrid_count=<Number of mrpt::slam::CWirelessPowerGridMap2D maps>
446  * landmarksMap_count=<0 or 1, for creating a mrpt::slam::CLandmarksMap map>
447  * beaconMap_count=<0 or 1, for creating a mrpt::slam::CBeaconMap map>
448  * pointsMap_count=<Number of mrpt::slam::CSimplePointsMap map>
449  * heightMap_count=<Number of mrpt::slam::CHeightGridMap2D maps>
450  * reflectivityMap_count=<Number of mrpt::slam::CReflectivityGridMap2D maps>
451  * colourPointsMap_count=<0 or 1, for creating a mrpt::slam::CColouredPointsMap map>
452  * weightedPointsMap_count=<0 or 1, for creating a mrpt::slam::CWeightedPointsMap map>
453  *
454  * // Selection of map for likelihood. Either a numeric value or the textual enum
455  * // enum value of slam::CMultiMetricMap::TOptions::TMapSelectionForLikelihood (e.g: either "-1" or "fuseAll", ect...)
456  * likelihoodMapSelection = -1
457  *
458  * // Enables (1 or "true") / Disables (0 or "false") insertion into specific maps (Defaults are all "true"):
459  * enableInsertion_pointsMap=<0/1>
460  * enableInsertion_landmarksMap=<0/1>
461  * enableInsertion_gridMaps=<0/1>
462  * enableInsertion_gasGridMaps=<0/1>
463  * enableInsertion_wifiGridMaps=<0/1>
464  * enableInsertion_beaconMap=<0/1>
465  * enableInsertion_heightMap=<0/1>
466  * enableInsertion_reflectivityMap=<0/1>
467  * enableInsertion_colourPointsMap=<0/1>
468  * enableInsertion_weightedPointsMap=<0/1>
469  * enableInsertion_octoMaps=<0/1>
470  * enableInsertion_colourOctoMaps=<0/1>
471  *
472  * // ====================================================
473  * // Creation Options for OccupancyGridMap ##:
474  * [<sectionName>+"_occupancyGrid_##_creationOpts"]
475  * min_x=<value>
476  * max_x=<value>
477  * min_y=<value>
478  * max_y=<value>
479  * resolution=<value>
480  *
481  * // Insertion Options for OccupancyGridMap ##:
482  * [<sectionName>+"_occupancyGrid_##_insertOpts"]
483  * <See COccupancyGridMap2D::TInsertionOptions>
484  *
485  * // Likelihood Options for OccupancyGridMap ##:
486  * [<sectionName>+"_occupancyGrid_##_likelihoodOpts"]
487  * <See COccupancyGridMap2D::TLikelihoodOptions>
488  *
489  * // ====================================================
490  * // Creation Options for OctoMap ##:
491  * [<sectionName>+"_octoMap_##_creationOpts"]
492  * resolution=<value>
493  *
494  * // Insertion Options for OctoMap ##:
495  * [<sectionName>+"_octoMap_##_insertOpts"]
496  * <See COctoMap::TInsertionOptions>
497  *
498  * // Likelihood Options for OctoMap ##:
499  * [<sectionName>+"_octoMap_##_likelihoodOpts"]
500  * <See COctoMap::TLikelihoodOptions>
501  *
502  * // ====================================================
503  * // Creation Options for ColourOctoMap ##:
504  * [<sectionName>+"_colourOctoMap_##_creationOpts"]
505  * resolution=<value>
506  *
507  * // Insertion Options for ColourOctoMap ##:
508  * [<sectionName>+"_colourOctoMap_##_insertOpts"]
509  * <See CColourOctoMap::TInsertionOptions>
510  *
511  * // Likelihood Options for ColourOctoMap ##:
512  * [<sectionName>+"_colourOctoMap_##_likelihoodOpts"]
513  * <See CColourOctoMap::TLikelihoodOptions>
514  *
515  *
516  * // ====================================================
517  * // Insertion Options for CSimplePointsMap ##:
518  * [<sectionName>+"_pointsMap_##_insertOpts"]
519  * <See CPointsMap::TInsertionOptions>
520  *
521  * // Likelihood Options for CSimplePointsMap ##:
522  * [<sectionName>+"_pointsMap_##_likelihoodOpts"]
523  * <See CPointsMap::TLikelihoodOptions>
524  *
525  *
526  * // ====================================================
527  * // Creation Options for CGasConcentrationGridMap2D ##:
528  * [<sectionName>+"_gasGrid_##_creationOpts"]
529  * mapType= <0-1> ; See CGasConcentrationGridMap2D::CGasConcentrationGridMap2D
530  * min_x=<value>
531  * max_x=<value>
532  * min_y=<value>
533  * max_y=<value>
534  * resolution=<value>
535  *
536  * // Insertion Options for CGasConcentrationGridMap2D ##:
537  * [<sectionName>+"_gasGrid_##_insertOpts"]
538  * <See CGasConcentrationGridMap2D::TInsertionOptions>
539 
540 
541 
542 
543  * // ====================================================
544  * // Creation Options for CWirelessPowerGridMap2D ##:
545  * [<sectionName>+"_wifiGrid_##_creationOpts"]
546  * mapType= <0-1> ; See CWirelessPowerGridMap2D::CWirelessPowerGridMap2D
547  * min_x=<value>
548  * max_x=<value>
549  * min_y=<value>
550  * max_y=<value>
551  * resolution=<value>
552  *
553  * // Insertion Options for CWirelessPowerGridMap2D ##:
554  * [<sectionName>+"_wifiGrid_##_insertOpts"]
555  * <See CWirelessPowerGridMap2D::TInsertionOptions>
556 
557 
558  *
559  *
560  * // ====================================================
561  * // Creation Options for CLandmarksMap ##:
562  * [<sectionName>+"_landmarksMap_##_creationOpts"]
563  * nBeacons=<# of beacons>
564  * beacon_001_ID=67 ; The ID and 3D coordinates of each beacon
565  * beacon_001_X=<x>
566  * beacon_001_Y=<x>
567  * beacon_001_Z=<x>
568  *
569  * // Insertion Options for CLandmarksMap ##:
570  * [<sectionName>+"_landmarksMap_##_insertOpts"]
571  * <See CLandmarksMap::TInsertionOptions>
572  *
573  * // Likelihood Options for CLandmarksMap ##:
574  * [<sectionName>+"_landmarksMap_##_likelihoodOpts"]
575  * <See CLandmarksMap::TLikelihoodOptions>
576  *
577  *
578  * // ====================================================
579  * // Insertion Options for CBeaconMap ##:
580  * [<sectionName>+"_beaconMap_##_insertOpts"]
581  * <See CBeaconMap::TInsertionOptions>
582  *
583  * // Likelihood Options for CBeaconMap ##:
584  * [<sectionName>+"_beaconMap_##_likelihoodOpts"]
585  * <See CBeaconMap::TLikelihoodOptions>
586  *
587  * // ====================================================
588  * // Creation Options for HeightGridMap ##:
589  * [<sectionName>+"_heightGrid_##_creationOpts"]
590  * mapType= <0-1> // See CHeightGridMap2D::CHeightGridMap2D
591  * min_x=<value>
592  * max_x=<value>
593  * min_y=<value>
594  * max_y=<value>
595  * resolution=<value>
596  *
597  * // Insertion Options for HeightGridMap ##:
598  * [<sectionName>+"_heightGrid_##_insertOpts"]
599  * <See CHeightGridMap2D::TInsertionOptions>
600  *
601  *
602  * // ====================================================
603  * // Creation Options for ReflectivityGridMap ##:
604  * [<sectionName>+"_reflectivityGrid_##_creationOpts"]
605  * min_x=<value> // See CReflectivityGridMap2D::CReflectivityGridMap2D
606  * max_x=<value>
607  * min_y=<value>
608  * max_y=<value>
609  * resolution=<value>
610  *
611  * // Insertion Options for HeightGridMap ##:
612  * [<sectionName>+"_reflectivityGrid_##_insertOpts"]
613  * <See CReflectivityGridMap2D::TInsertionOptions>
614  *
615  *
616  * // ====================================================
617  * // Insertion Options for CColouredPointsMap ##:
618  * [<sectionName>+"_colourPointsMap_##_insertOpts"]
619  * <See CPointsMap::TInsertionOptions>
620  *
621  *
622  * // Color Options for CColouredPointsMap ##:
623  * [<sectionName>+"_colourPointsMap_##_colorOpts"]
624  * <See CColouredPointsMap::TColourOptions>
625  *
626  * // Likelihood Options for CSimplePointsMap ##:
627  * [<sectionName>+"_colourPointsMap_##_likelihoodOpts"]
628  * <See CPointsMap::TLikelihoodOptions>
629  *
630  *
631  * // ====================================================
632  * // Insertion Options for CWeightedPointsMap ##:
633  * [<sectionName>+"_weightedPointsMap_##_insertOpts"]
634  * <See CPointsMap::TInsertionOptions>
635  *
636  *
637  * // Likelihood Options for CWeightedPointsMap ##:
638  * [<sectionName>+"_weightedPointsMap_##_likelihoodOpts"]
639  * <See CPointsMap::TLikelihoodOptions>
640  *
641  * \endcode
642  *
643  * Where:
644  * - ##: Represents the index of the map (e.g. "00","01",...)
645  * - By default, the variables into each "TOptions" structure of the maps are defined in textual form by the same name of the corresponding C++ variable (e.g. "float resolution;" -> "resolution=0.10")
646  *
647  * \note Examples of map definitions can be found in the '.ini' files provided in the demo directories: "share/mrpt/config-files/"
648  */
649  void loadFromConfigFile(
650  const mrpt::utils::CConfigFileBase &source,
651  const std::string &sectionName);
652 
653  /** This method dumps the options of the multi-metric map AND those of every internal map.
654  */
655  void dumpToTextStream(CStream &out) const;
656  };
657 
658  } // End of namespace
659 
660 
661  // Specializations MUST occur at the same namespace:
662  namespace utils
663  {
664  template <>
666  {
668  static void fill(bimap<enum_t,std::string> &m_map)
669  {
674  m_map.insert(slam::CMultiMetricMap::TOptions::mapPoints, "mrSimpleAverage");
683  }
684  };
685  } // End of namespace
686 
687 
688 
689 } // End of namespace
690 
691 #endif
CPointsMap::TLikelihoodOptions likelihoodOpts
//!< Customizable initial likelihood options
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
bool enableInsertion_colourPointsMaps
Default = true (set to false to avoid "insertObservation" to update a given map)
Specific options for landmarks maps (mrpt::slam::CLandmarksMap)
float resolution
See CHeightGridMap2D::CHeightGridMap2D.
COccupancyGridMap2D::TLikelihoodOptions likelihoodOpts
Customizable initial options.
CGasConcentrationGridMap2D::TMapRepresentation mapType
The kind of map representation (see CGasConcentrationGridMap2D::CGasConcentrationGridMap2D) ...
double resolution
The finest resolution of the octomap (default: 0.10 meters)
CWirelessPowerGridMap2D::TMapRepresentation mapType
The kind of map representation (see CWirelessPowerGridMap2D::CWirelessPowerGridMap2D) ...
std::deque< CWirelessPowerGridMap2DPtr > m_wifiGridMaps
CLandmarksMap::TLikelihoodOptions likelihoodOpts
Customizable initial options.
The definition of parameters for generating colors from laser scans.
CHeightGridMap2D::TMapRepresentation mapType
The kind of map representation (see CHeightGridMap2D::CHeightGridMap2D)
This class stores any customizable set of metric maps.
Specific options for 2D grid maps (mrpt::slam::COccupancyGridMap2D)
With this struct options are provided to the observation insertion process.
bool enableInsertion_weightedPointsMaps
Default = true (set to false to avoid "insertObservation" to update a given map)
Scalar * iterator
Definition: eigen_plugins.h:23
COctoMap::TInsertionOptions insertionOpts
Customizable initial options.
Options used when evaluating "computeObservationLikelihood" in the derived classes.
Definition: CPointsMap.h:193
bool enableInsertion_wifiGridMaps
Default = true (set to false to avoid "insertObservation" to update a given map)
CHeightGridMap2D::TInsertionOptions insertionOpts
Customizable initial options.
std::deque< CGasConcentrationGridMap2DPtr > m_gasGridMaps
bool enableInsertion_beaconMap
Default = true (set to false to avoid "insertObservation" to update a given map)
STL namespace.
With this struct options are provided to the observation insertion process.
Definition: CPointsMap.h:165
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
std::deque< CReflectivityGridMap2DPtr > m_reflectivityMaps
CColouredOctoMap::TInsertionOptions insertionOpts
Customizable initial options.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
CPointsMap::TInsertionOptions insertionOpts
Customizable initial options for loading the class' own defaults.
std::deque< TMetricMapInitializer >::const_iterator const_iterator
unsigned int m_ID
An auxiliary variable that can be used freely by the users (this will be copied to other maps using t...
std::deque< CColouredOctoMapPtr > m_colourOctoMaps
CMultiMetricMap::TOptions options
This options will be loaded when creating the set of maps in CMultiMetricMap (See CMultiMetricMap::TO...
This struct contains data for choosing the method by which new beacons are inserted in the map...
Definition: CBeaconMap.h:149
With this struct options are provided to the likelihood computations.
float resolution
See CWirelessPowerGridMap2D::CWirelessPowerGridMap2D.
COccupancyGridMap2D::TInsertionOptions insertionOpts
Customizable initial options.
Parameters related with inserting observations into the map:
CColouredPointsMap::TColourOptions colourOpts
Customizable initial options for loading the class' own defaults. */.
With this struct options are provided to the observation insertion process.
CPointsMap::TLikelihoodOptions likelihoodOpts
//!< Customizable initial likelihood options
This class allows loading and storing values and vectors of different types from a configuration text...
CPointsMap::TInsertionOptions insertionOpts
Customizable initial options for loading the class' own defaults.
CPointsMap::TInsertionOptions insertionOpts
Customizable initial options for loading the class' own defaults.
std::deque< CMultiMetricMap::TPairIdBeacon > initialBeacons
Initial contents of the map, especified by a set of 3D Beacons with associated IDs.
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:83
CBeaconMap::TInsertionOptions insertionOpts
Customizable initial options.
float resolution
See CReflectivityGridMap2DOptions::CReflectivityGridMap2DOptions.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
CColouredPointsMapPtr m_colourPointsMap
bool enableInsertion_gasGridMaps
Default = true (set to false to avoid "insertObservation" to update a given map)
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
float resolution
See CGasConcentrationGridMap2D::CGasConcentrationGridMap2D.
Parameters related with inserting observations into the map:
Specific options for wifi grid maps (mrpt::slam::CWirelessPowerGridMap2D)
A list of TMatchingPair.
Definition: TMatchingPair.h:66
Declares a class that represents any robot's observation.
Definition: CObservation.h:52
A bidirectional version of std::map, declared as bimap and which actually contains two std...
Definition: bimap.h:27
std::deque< COccupancyGridMap2DPtr > m_gridMaps
Specific options for coloured point maps (mrpt::slam::CPointsMap)
std::deque< TMetricMapInitializer >::iterator iterator
bool enableInsertion_colourOctoMaps
Default = true (set to false to avoid "insertObservation" to update a given map)
TMapSelectionForLikelihood
This selects the map to be used when computing the likelihood of an observation.
Parameters for the determination of matchings between point clouds, etc.
Definition: CMetricMap.h:32
CWirelessPowerGridMap2D::TInsertionOptions insertionOpts
Customizable initial options.
bool enableInsertion_octoMaps
Default = true (set to false to avoid "insertObservation" to update a given map)
With this struct options are provided to the likelihood computations.
Definition: CBeaconMap.h:124
double resolution
The finest resolution of the octomap (default: 0.10 meters)
std::deque< TMetricMapInitializer > m_list
bool enableInsertion_reflectivityMaps
Default = true (set to false to avoid "insertObservation" to update a given map)
CPointsMap::TLikelihoodOptions likelihoodOpts
//!< Customizable initial likelihood options
A class used to store a 3D point.
Definition: CPoint3D.h:32
CBeaconMap::TLikelihoodOptions likelihoodOpts
Customizable initial options.
std::deque< CHeightGridMap2DPtr > m_heightMaps
float resolution
See COccupancyGridMap2D::COccupancyGridMap2D.
Specific options for landmarks maps (mrpt::slam::CBeaconMap)
bool enableInsertion_gridMaps
Default = true (set to false to avoid "insertObservation" to update a given map)
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
CColouredOctoMap::TLikelihoodOptions likelihoodOpts
Customizable initial options.
Parameters related with inserting observations into the map.
Specific options for 3D octo maps (mrpt::slam::COctoMap)
Specific options for gas grid maps (mrpt::slam::CGasConcentrationGridMap2D)
Specific options for 3D octo maps (mrpt::slam::COctoMap)
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
bool enableInsertion_landmarksMap
Default = true (set to false to avoid "insertObservation" to update a given map)
A class used to store a 2D pose.
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:69
Specific options for height grid maps (mrpt::slam::CHeightGridMap2D)
CReflectivityGridMap2D::TInsertionOptions insertionOpts
Customizable initial options.
CLandmarksMap::TInsertionOptions insertionOpts
Customizable initial options.
Specific options for coloured point maps (mrpt::slam::CPointsMap)
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
Definition: CMetricMap.h:55
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans or other sensors...
Definition: CPointsMap.h:59
Parameters related with inserting observations into the map.
Specific options for point maps (mrpt::slam::CPointsMap)
std::pair< CPoint3D, unsigned int > TPairIdBeacon
bool enableInsertion_pointsMap
Default = true (set to false to avoid "insertObservation" to update a given map)
TRuntimeClassIdPtr metricMapClassType
Set this to CLASS_ID(< class >) where < class > is any CMetricMap derived class.
CWeightedPointsMapPtr m_weightedPointsMap
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:68
CLandmarksMapPtr m_landmarksMap
Specific options for height grid maps (mrpt::slam::CReflectivityGridMap2D)
TMapRepresentation
The type of map representation to be used.
COctoMap::TLikelihoodOptions likelihoodOpts
Customizable initial options.
Some options for this class:
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
bool m_disableSaveAs3DObject
This value will be copied to the member with the same value in the map, see mrpt::slam::CMetricMap::m...
CGasConcentrationGridMap2D::TInsertionOptions insertionOpts
Customizable initial options.
std::deque< COctoMapPtr > m_octoMaps
Each structure of this kind will determine the kind (and initial configuration) of one map to be buil...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
std::deque< CSimplePointsMapPtr > m_pointsMaps
void push_back(const TMetricMapInitializer &o)
bool enableInsertion_heightMaps
Default = true (set to false to avoid "insertObservation" to update a given map)
With this struct options are provided to the observation likelihood computation process.



Page generated by Doxygen 1.8.8 for MRPT 1.2.2 SVN:Unversioned directory at Tue Oct 14 02:14:08 UTC 2014