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