Main MRPT website > C++ reference for MRPT 1.5.6
maps/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-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 CMultiMetricMap_H
10 #define CMultiMetricMap_H
11 
13 #include <mrpt/maps/COctoMap.h>
24 #include <mrpt/maps/CBeaconMap.h>
25 #include <mrpt/maps/CMetricMap.h>
28 #include <mrpt/utils/TEnumType.h>
30 #include <mrpt/obs/obs_frwds.h>
31 
32 #include <mrpt/slam/link_pragmas.h>
33 
34 namespace mrpt
35 {
36 namespace maps
37 {
38  class TSetOfMetricMapInitializers;
39 
41 
42  /** This class stores any customizable set of metric maps.
43  * The internal metric maps can be accessed directly by the user as smart pointers with CMultiMetricMap::getMapByIndex() or via `iterator`s.
44  * The utility of this container is to operate on several maps simultaneously: update them by inserting observations,
45  * evaluate the likelihood of one observation by fusing (multiplying) the likelihoods over the different maps, etc.
46  *
47  * <b>These kinds of metric maps can be kept inside</b> (list may be incomplete, refer to classes derived from mrpt::maps::CMetricMap):
48  * - mrpt::maps::CSimplePointsMap: For 2D or 3D range scans, ...
49  * - mrpt::maps::COccupancyGridMap2D: 2D, <b>horizontal</b> laser range scans, at different altitudes.
50  * - mrpt::maps::COctoMap: For 3D occupancy grids of variable resolution, with octrees (based on the library `octomap`).
51  * - mrpt::maps::CColouredOctoMap: The same than above, but nodes can store RGB data appart from occupancy.
52  * - mrpt::maps::CLandmarksMap: For visual landmarks,etc...
53  * - mrpt::maps::CGasConcentrationGridMap2D: For gas concentration maps.
54  * - mrpt::maps::CWirelessPowerGridMap2D: For wifi power maps.
55  * - mrpt::maps::CBeaconMap: For range-only SLAM.
56  * - mrpt::maps::CHeightGridMap2D: For elevation maps of height for each (x,y) location (Digital elevation model, DEM)
57  * - mrpt::maps::CHeightGridMap2D_MRF: DEMs as Markov Random Field (MRF)
58  * - mrpt::maps::CReflectivityGridMap2D: For maps of "reflectivity" for each (x,y) location.
59  * - mrpt::maps::CColouredPointsMap: For point map with color.
60  * - mrpt::maps::CWeightedPointsMap: For point map with weights (capable of "fusing").
61  *
62  * See CMultiMetricMap::setListOfMaps() for the method for initializing this class programmatically.
63  * See also TSetOfMetricMapInitializers::loadFromConfigFile for a template of ".ini"-like configuration
64  * file that can be used to define which maps to create and all their parameters.
65  * Alternatively, the list of maps is public so it can be directly manipulated/accessed in CMultiMetricMap::maps
66  *
67  * Configuring the list of maps: Alternatives
68  * --------------------------------------------
69  *
70  * **Method #1: Using map definition structures**
71  * \code
72  * mrpt::maps::TSetOfMetricMapInitializers map_inits;
73  * {
74  * mrpt::maps::COccupancyGridMap2D::TMapDefinition def;
75  * def.resolution = 0.05;
76  * def.insertionOpts.maxOccupancyUpdateCertainty = 0.8;
77  * def.insertionOpts.maxDistanceInsertion = 30;
78  * map_inits.push_back(def);
79  * }
80  * {
81  * mrpt::maps::CSimplePointsMap::TMapDefinition def;
82  * map_inits.push_back(def);
83  * }
84  * mrpt::maps::CMultiMetricMap theMap;
85  * theMap.setListOfMaps(map_inits);
86  * \endcode
87  *
88  * **Method #2: Using a configuration file**
89  * See TSetOfMetricMapInitializers::loadFromConfigFile() for details on expected file format.
90  *
91  * \code
92  * mrpt::utils::CConfigFile cfgFile("file.cfg");
93  * mrpt::maps::TSetOfMetricMapInitializers map_inits;
94  * map_inits.loadFromConfigFile(cfgFile, "MapDefinition");
95  *
96  * mrpt::maps::CMultiMetricMap theMap;
97  * theMap.setListOfMaps(map_inits);
98  * \endcode
99  *
100  * **Method #3: Manual manipulation**
101  *
102  * \code
103  * mrpt::maps::CMultiMetricMap theMap;
104  * {
105  * mrpt::maps::CSimplePointsMapPtr ptMap = mrpt::maps::CSimplePointsMap::Create();
106  * theMap.maps.push_back(ptMap);
107  * }
108  * \endcode
109  *
110  * \note [New in MRPT 1.3.0]: `likelihoodMapSelection`, which selected the map to be used when
111  * computing the likelihood of an observation, has been removed. Use the `enableObservationLikelihood`
112  * property of each individual map declaration.
113  *
114  * \note [New in MRPT 1.3.0]: `enableInsertion_{pointsMap,...}` have been also removed.
115  * Use the `enableObservationInsertion` property of each map declaration.
116  *
117  * \note [New in MRPT 1.3.0]: Plain list of maps is exposed in `maps` member. Proxies named `m_pointsMaps`,`m_gridMaps`, etc.
118  * are provided for backwards-compatibility and for their utility.
119  *
120  * \note This class belongs to [mrpt-slam] instead of [mrpt-maps] due to the dependency on map classes in mrpt-vision.
121  * \sa CMetricMap \ingroup mrpt_slam_grp
122  */
123  class SLAM_IMPEXP CMultiMetricMap : public mrpt::maps::CMetricMap
124  {
125  // This must be added to any CSerializable derived class:
127  protected:
128  void deleteAllMaps(); //!< Deletes all maps and clears the internal lists of maps (with clear_unique(), so user copies remain alive)
129  void internal_clear() MRPT_OVERRIDE; //!< Clear all elements of the map.
130  // See base class docs
131  bool internal_insertObservation( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
132  /** Returns true if any of the inner maps is able to compute a sensible likelihood function for this observation.
133  * \param obs The observation.
134  * \sa computeObservationLikelihood
135  */
136  bool internal_canComputeObservationLikelihood( const mrpt::obs::CObservation *obs ) const MRPT_OVERRIDE;
137  // See docs in base class
138  double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) MRPT_OVERRIDE;
139 
140  public:
141  /** @name Access to internal list of maps: direct list, iterators, utility methods and proxies
142  @{ */
143  typedef std::deque< mrpt::utils::poly_ptr_ptr<mrpt::maps::CMetricMapPtr> > TListMaps;
144 
145  /** The list of MRPT metric maps in this object. Use dynamic_cast or smart pointer-based downcast to access maps by their actual type.
146  * You can directly manipulate this list. Helper methods to initialize it are described in the docs of CMultiMetricMap
147  */
148  TListMaps maps;
149 
150  typedef TListMaps::iterator iterator;
151  typedef TListMaps::const_iterator const_iterator;
152  iterator begin() { return maps.begin(); }
153  const_iterator begin() const { return maps.begin(); }
154  iterator end() { return maps.end(); }
155  const_iterator end() const { return maps.end(); }
156 
157  /** Gets the i-th map \exception std::runtime_error On out-of-bounds */
158  mrpt::maps::CMetricMapPtr getMapByIndex(size_t idx) const;
159 
160  /** Returns the i'th observation of a given class (or of a descendant class), or NULL if there is no such observation in the array.
161  * Example:
162  * \code
163  * CObservationImagePtr obs = m_SF->getObservationByClass<CObservationImage>();
164  * \endcode
165  * By default (ith=0), the first observation is returned.
166  */
167  template <typename T>
168  typename T::Ptr getMapByClass( const size_t &ith = 0 ) const
169  {
170  size_t foundCount = 0;
171  const mrpt::utils::TRuntimeClassId* class_ID = T::classinfo;
172  for (const_iterator it = begin();it!=end();++it)
173  if ( (*it)->GetRuntimeClass()->derivedFrom( class_ID ) )
174  if (foundCount++ == ith)
175  return typename T::Ptr(it->get_ptr());
176  return typename T::Ptr(); // Not found: return empty smart pointer
177  }
178 
179  /** Takes a const ref of a STL non-associative container of smart pointers at construction and exposes an interface
180  * mildly similar to that of another STL container containing only those elements
181  * in the original container that can be `dynamic_cast`ed to `SELECTED_CLASS_PTR` */
182  template <class SELECTED_CLASS_PTR, class CONTAINER>
184  {
185  typedef typename SELECTED_CLASS_PTR::value_type* ptr_t;
186  typedef const typename SELECTED_CLASS_PTR::value_type* const_ptr_t;
187  ProxyFilterContainerByClass(CONTAINER &source) : m_source(&source) {}
188  ProxyFilterContainerByClass(ProxyFilterContainerByClass<SELECTED_CLASS_PTR, CONTAINER>& ) : m_source() {} // m_source init in parent copy ctor
189 
191 #if (__cplusplus>199711L)
192  ProxyFilterContainerByClass(ProxyFilterContainerByClass<SELECTED_CLASS_PTR, CONTAINER>&& ) : m_source() {} // m_source init in parent copy ctor
193  ProxyFilterContainerByClass<SELECTED_CLASS_PTR, CONTAINER>& operator=(ProxyFilterContainerByClass<SELECTED_CLASS_PTR, CONTAINER>&&o) { return *this; } // Do nothing, we must keep refs to our own parent
194 #endif
195  bool empty() const { return size()==0; }
196  size_t size() const {
197  size_t cnt=0;
198  for(typename CONTAINER::const_iterator it=m_source->begin();it!=m_source->end();++it)
199  if ( dynamic_cast<const_ptr_t>(it->pointer()) )
200  cnt++;
201  return cnt;
202  }
203  SELECTED_CLASS_PTR operator [](size_t index) const {
204  size_t cnt=0;
205  for(typename CONTAINER::const_iterator it=m_source->begin();it!=m_source->end();++it)
206  if ( dynamic_cast<const_ptr_t>(it->pointer()) )
207  if (cnt++ == index) { return SELECTED_CLASS_PTR(it->get_ptr()); }
208  throw std::out_of_range("Index is out of range");
209  }
210  template <typename ELEMENT>
211  void push_back(const ELEMENT &element) { m_source->push_back(element); }
212  private:
213  CONTAINER * m_source;
214  }; // end ProxyFilterContainerByClass
215 
216  /** A proxy like ProxyFilterContainerByClass, but it directly appears as if
217  * it was a single smart pointer (empty if no matching object is found in the container) */
218  template <class SELECTED_CLASS_PTR, class CONTAINER>
220  {
221  typedef typename SELECTED_CLASS_PTR::value_type pointee_t;
222  typedef typename SELECTED_CLASS_PTR::value_type* ptr_t;
223  typedef const typename SELECTED_CLASS_PTR::value_type* const_ptr_t;
224  ProxySelectorContainerByClass(CONTAINER &source) : m_source(&source) {}
227 #if (__cplusplus>199711L)
228  ProxySelectorContainerByClass(ProxySelectorContainerByClass<SELECTED_CLASS_PTR, CONTAINER>&& ) : m_source() {} // m_source init in parent copy ctor
229  ProxySelectorContainerByClass<SELECTED_CLASS_PTR, CONTAINER>& operator=(ProxySelectorContainerByClass<SELECTED_CLASS_PTR, CONTAINER>&&o) { return *this; } // Do nothing, we must keep refs to our own parent
230 #endif
231 
232  operator const SELECTED_CLASS_PTR & () const { internal_update_ref(); return m_ret; }
233  operator bool() const { internal_update_ref(); return m_ret.present(); }
234  bool present() const { internal_update_ref(); return m_ret.present(); }
235  ptr_t pointer(){ internal_update_ref(); return m_ret.pointer(); }
236  ptr_t operator ->() const {
237  internal_update_ref();
238  if (m_ret.present()) return m_ret.pointer();
239  else throw std::runtime_error("Tried to derefer NULL pointer");
240  }
241  pointee_t & operator *() const {
242  internal_update_ref();
243  if (m_ret.present()) return *m_ret.pointer();
244  else throw std::runtime_error("Tried to derefer NULL pointer");
245  }
246  private:
247  CONTAINER * m_source;
248  mutable SELECTED_CLASS_PTR m_ret;
249  void internal_update_ref() const {
250  for(typename CONTAINER::const_iterator it=m_source->begin();it!=m_source->end();++it) {
251  if ( dynamic_cast<const_ptr_t>(it->pointer()) ) {
252  m_ret=SELECTED_CLASS_PTR(it->get_ptr());
253  return;
254  }
255  }
256  m_ret=SELECTED_CLASS_PTR(); // Not found
257  }
258 
259  }; // end ProxySelectorContainerByClass
260 
261  ProxyFilterContainerByClass<mrpt::maps::CSimplePointsMapPtr,TListMaps> m_pointsMaps; //!< STL-like proxy to access this kind of maps in \ref maps
262  ProxyFilterContainerByClass<mrpt::maps::COccupancyGridMap2DPtr,TListMaps> m_gridMaps; //!< STL-like proxy to access this kind of maps in \ref maps
263  ProxyFilterContainerByClass<mrpt::maps::COctoMapPtr,TListMaps> m_octoMaps; //!< STL-like proxy to access this kind of maps in \ref maps
264  ProxyFilterContainerByClass<mrpt::maps::CColouredOctoMapPtr,TListMaps> m_colourOctoMaps; //!< STL-like proxy to access this kind of maps in \ref maps
265  ProxyFilterContainerByClass<mrpt::maps::CGasConcentrationGridMap2DPtr,TListMaps> m_gasGridMaps; //!< STL-like proxy to access this kind of maps in \ref maps
266  ProxyFilterContainerByClass<mrpt::maps::CWirelessPowerGridMap2DPtr,TListMaps> m_wifiGridMaps; //!< STL-like proxy to access this kind of maps in \ref maps
267  ProxyFilterContainerByClass<mrpt::maps::CHeightGridMap2DPtr,TListMaps> m_heightMaps; //!< STL-like proxy to access this kind of maps in \ref maps
268  ProxyFilterContainerByClass<mrpt::maps::CHeightGridMap2D_MRFPtr,TListMaps> m_heightMRFMaps; //!< STL-like proxy to access this kind of maps in \ref maps
269  ProxyFilterContainerByClass<mrpt::maps::CReflectivityGridMap2DPtr,TListMaps> m_reflectivityMaps; //!< STL-like proxy to access this kind of maps in \ref maps
270  ProxySelectorContainerByClass<mrpt::maps::CColouredPointsMapPtr,TListMaps> m_colourPointsMap; //!< Proxy that looks like a smart pointer to the first matching object in \ref maps
271  ProxySelectorContainerByClass<mrpt::maps::CWeightedPointsMapPtr,TListMaps> m_weightedPointsMap; //!< Proxy that looks like a smart pointer to the first matching object in \ref maps
272  ProxySelectorContainerByClass<mrpt::maps::CLandmarksMapPtr,TListMaps> m_landmarksMap; //!< Proxy that looks like a smart pointer to the first matching object in \ref maps
273  ProxySelectorContainerByClass<mrpt::maps::CBeaconMapPtr,TListMaps> m_beaconMap; //!< Proxy that looks like a smart pointer to the first matching object in \ref maps
274 
275  /** @} */
276 
277  /** Constructor.
278  * \param initializers One internal map will be created for each entry in this "TSetOfMetricMapInitializers" struct.
279  * If initializers is NULL, no internal map will be created.
280  */
281  CMultiMetricMap(const mrpt::maps::TSetOfMetricMapInitializers *initializers = NULL);
282 
284  CMultiMetricMap& operator =(const CMultiMetricMap &o);
285 
286 #if (__cplusplus>199711L)
288  CMultiMetricMap& operator =(CMultiMetricMap &&o);
289 #endif
290 
291  /** Sets the list of internal map according to the passed list of map initializers (Current maps' content will be deleted!) */
292  void setListOfMaps( const mrpt::maps::TSetOfMetricMapInitializers *initializers );
293  /** \overload */
294  void setListOfMaps( const mrpt::maps::TSetOfMetricMapInitializers &initializers ) { this->setListOfMaps(&initializers); }
295 
296  bool isEmpty() const MRPT_OVERRIDE; //!< Returns true if all maps returns true to their isEmpty() method, which is map-dependent. Read the docs of each map class
297 
298  // See docs in base class.
299  virtual void determineMatching2D(
300  const mrpt::maps::CMetricMap * otherMap,
301  const mrpt::poses::CPose2D & otherMapPose,
302  mrpt::utils::TMatchingPairList & correspondences,
303  const mrpt::maps::TMatchingParams & params,
304  mrpt::maps::TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
305 
306  /** See the definition in the base class: Calls in this class become a call to every single map in this set. */
307  float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
308 
309  /** The implementation in this class just calls all the corresponding method of the contained metric maps */
310  void saveMetricMapRepresentationToFile(const std::string &filNamePrefix ) const MRPT_OVERRIDE;
311 
312  /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
313  * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
314  */
315  void auxParticleFilterCleanUp() MRPT_OVERRIDE;
316 
317  /** Returns a 3D object representing the map.
318  */
319  void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE;
320 
321  /** If the map is a simple point map or it's a multi-metric map that contains EXACTLY one simple point map, return it.
322  * Otherwise, return NULL
323  */
324  virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const MRPT_OVERRIDE;
325  virtual mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() MRPT_OVERRIDE;
326 
327  /** 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.
328  */
329  unsigned int m_ID;
330 
331  }; // End of class def.
333 
334 
335  } // End of namespace
336 } // End of namespace
337 
338 #endif
std::deque< mrpt::utils::poly_ptr_ptr< mrpt::maps::CMetricMapPtr > > TListMaps
TListMaps::const_iterator const_iterator
Parameters for CMetricMap::compute3DMatchingRatio()
const_iterator end() const
ProxySelectorContainerByClass< mrpt::maps::CWeightedPointsMapPtr, TListMaps > m_weightedPointsMap
Proxy that looks like a smart pointer to the first matching object in maps.
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &initializers)
ProxyFilterContainerByClass< mrpt::maps::COctoMapPtr, TListMaps > m_octoMaps
STL-like proxy to access this kind of maps in maps.
T::Ptr getMapByClass(const size_t &ith=0) const
Returns the i'th observation of a given class (or of a descendant class), or NULL if there is no such...
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
ProxyFilterContainerByClass< mrpt::maps::CHeightGridMap2D_MRFPtr, TListMaps > m_heightMRFMaps
STL-like proxy to access this kind of maps in maps.
const Scalar * const_iterator
Definition: eigen_plugins.h:24
Takes a const ref of a STL non-associative container of smart pointers at construction and exposes an...
const_iterator begin() const
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2DPtr, TListMaps > m_gridMaps
STL-like proxy to access this kind of maps in maps.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
ProxySelectorContainerByClass< mrpt::maps::CBeaconMapPtr, TListMaps > m_beaconMap
Proxy that looks like a smart pointer to the first matching object in maps.
#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...
GLuint GLuint end
Definition: glew.h:1167
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
ProxyFilterContainerByClass< mrpt::maps::CGasConcentrationGridMap2DPtr, TListMaps > m_gasGridMaps
STL-like proxy to access this kind of maps in maps.
ProxySelectorContainerByClass< mrpt::maps::CLandmarksMapPtr, TListMaps > m_landmarksMap
Proxy that looks like a smart pointer to the first matching object in maps.
VALUE & operator[](const KEY &key)
Write/read via [i] operator, that creates an element if it didn't exist already.
Definition: ts_hash_map.h:123
ProxySelectorContainerByClass(ProxySelectorContainerByClass< SELECTED_CLASS_PTR, CONTAINER > &)
ProxyFilterContainerByClass< mrpt::maps::CWirelessPowerGridMap2DPtr, TListMaps > m_wifiGridMaps
STL-like proxy to access this kind of maps in maps.
ProxyFilterContainerByClass< mrpt::maps::CColouredOctoMapPtr, TListMaps > m_colourOctoMaps
STL-like proxy to access this kind of maps in maps.
ProxyFilterContainerByClass< mrpt::maps::CHeightGridMap2DPtr, TListMaps > m_heightMaps
STL-like proxy to access this kind of maps in maps.
GLsizeiptr size
Definition: glew.h:1586
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
mrpt::maps::CMultiMetricMap CMultiMetricMap
Backward compatible typedef.
GLuint index
Definition: glew.h:1721
Declares a virtual base class for all metric maps storage classes.
ProxySelectorContainerByClass< SELECTED_CLASS_PTR, CONTAINER > & operator=(const ProxySelectorContainerByClass< SELECTED_CLASS_PTR, CONTAINER > &o)
ProxyFilterContainerByClass< SELECTED_CLASS_PTR, CONTAINER > & operator=(const ProxyFilterContainerByClass< SELECTED_CLASS_PTR, CONTAINER > &o)
GLfloat * params
Definition: glew.h:1436
A proxy like ProxyFilterContainerByClass, but it directly appears as if it was a single smart pointer...
ProxyFilterContainerByClass(ProxyFilterContainerByClass< SELECTED_CLASS_PTR, CONTAINER > &)
A structure that holds runtime class type information.
Definition: CObject.h:46
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMapPtr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
This class stores any customizable set of metric maps.
ProxySelectorContainerByClass< mrpt::maps::CColouredPointsMapPtr, TListMaps > m_colourPointsMap
Proxy that looks like a smart pointer to the first matching object in maps.
Parameters for the determination of matchings between point clouds, etc.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59
ProxyFilterContainerByClass< mrpt::maps::CReflectivityGridMap2DPtr, TListMaps > m_reflectivityMaps
STL-like proxy to access this kind of maps in maps.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018