Main MRPT website > C++ reference for MRPT 1.5.7
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  */
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 
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()
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&#39;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. ...
STL namespace.
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...
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...
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
GLuint index
Definition: glext.h:3891
ProxyFilterContainerByClass< mrpt::maps::CGasConcentrationGridMap2DPtr, TListMaps > m_gasGridMaps
STL-like proxy to access this kind of maps in maps.
GLuint GLuint end
Definition: glext.h:3512
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&#39;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.
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...
mrpt::maps::CMultiMetricMap CMultiMetricMap
Backward compatible typedef.
Declares a virtual base class for all metric maps storage classes.
ProxySelectorContainerByClass< SELECTED_CLASS_PTR, CONTAINER > & operator=(const ProxySelectorContainerByClass< SELECTED_CLASS_PTR, CONTAINER > &o)
GLsizei GLsizei GLchar * source
Definition: glext.h:3908
ProxyFilterContainerByClass< SELECTED_CLASS_PTR, CONTAINER > & operator=(const ProxyFilterContainerByClass< SELECTED_CLASS_PTR, CONTAINER > &o)
A proxy like ProxyFilterContainerByClass, but it directly appears as if it was a single smart pointer...
ProxyFilterContainerByClass(ProxyFilterContainerByClass< SELECTED_CLASS_PTR, CONTAINER > &)
GLsizeiptr size
Definition: glext.h:3779
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_)
GLenum const GLfloat * params
Definition: glext.h:3514
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59
const_iterator begin() const
ProxyFilterContainerByClass< mrpt::maps::CReflectivityGridMap2DPtr, TListMaps > m_reflectivityMaps
STL-like proxy to access this kind of maps in maps.



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019