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  */
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!) */
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
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#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...
Declares a virtual base class for all metric maps storage classes.
This class stores any customizable set of metric maps.
TListMaps::const_iterator const_iterator
ProxyFilterContainerByClass< mrpt::maps::COctoMapPtr, TListMaps > m_octoMaps
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.
virtual ~CMultiMetricMap()
Destructor.
ProxySelectorContainerByClass< mrpt::maps::CBeaconMapPtr, TListMaps > m_beaconMap
Proxy that looks like a smart pointer to the first matching object in maps.
ProxySelectorContainerByClass< mrpt::maps::CWeightedPointsMapPtr, TListMaps > m_weightedPointsMap
Proxy that looks like a smart pointer to the first matching object in maps.
ProxyFilterContainerByClass< mrpt::maps::CHeightGridMap2DPtr, TListMaps > m_heightMaps
STL-like proxy to access this kind of maps in maps.
ProxySelectorContainerByClass< mrpt::maps::CColouredPointsMapPtr, TListMaps > m_colourPointsMap
Proxy that looks like a smart pointer to the first matching object in maps.
ProxyFilterContainerByClass< mrpt::maps::CReflectivityGridMap2DPtr, TListMaps > m_reflectivityMaps
STL-like proxy to access this kind of maps in maps.
CMultiMetricMap(const mrpt::maps::TSetOfMetricMapInitializers *initializers=NULL)
Constructor.
mrpt::maps::CMetricMapPtr getMapByIndex(size_t idx) const
Gets the i-th map.
ProxyFilterContainerByClass< mrpt::maps::CSimplePointsMapPtr, TListMaps > m_pointsMaps
STL-like proxy to access this kind of maps in maps.
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &initializers)
This is an overloaded member function, provided for convenience. It differs from the above function o...
std::deque< mrpt::maps::CMetricMapPtr > TListMaps
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...
ProxyFilterContainerByClass< mrpt::maps::CWirelessPowerGridMap2DPtr, TListMaps > m_wifiGridMaps
STL-like proxy to access this kind of maps in maps.
CMultiMetricMap(const mrpt::maps::CMultiMetricMap &other)
Copy constructor.
void internal_clear() MRPT_OVERRIDE
Clear all elements of the map.
bool isEmpty() const MRPT_OVERRIDE
Returns true if all maps returns true to their isEmpty() method, which is map-dependent....
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers *initializers)
Sets the list of internal map according to the passed list of map initializers (Current maps' content...
void deleteAllMaps()
Deletes all maps and clears the internal lists of maps (with clear_unique(), so user copies remain al...
ProxyFilterContainerByClass< mrpt::maps::CGasConcentrationGridMap2DPtr, TListMaps > m_gasGridMaps
STL-like proxy to access this kind of maps in maps.
ProxyFilterContainerByClass< mrpt::maps::CHeightGridMap2D_MRFPtr, TListMaps > m_heightMRFMaps
STL-like proxy to access this kind of maps in maps.
ProxyFilterContainerByClass< mrpt::maps::COccupancyGridMap2DPtr, TListMaps > m_gridMaps
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.
const_iterator begin() const
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
const Scalar * const_iterator
Definition: eigen_plugins.h:24
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:26
EIGEN_STRONG_INLINE iterator end()
Definition: eigen_plugins.h:27
std::vector< T1 > operator*(const std::vector< T1 > &a, const std::vector< T2 > &b)
a*b (element-wise multiplication)
Definition: ops_vectors.h:59
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:38
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Takes a const ref of a STL non-associative container of smart pointers at construction and exposes an...
A proxy like ProxyFilterContainerByClass, but it directly appears as if it was a single smart pointer...
Additional results from the determination of matchings between point clouds, etc.,...
Parameters for the determination of matchings between point clouds, etc.
Parameters for CMetricMap::compute3DMatchingRatio()
A structure that holds runtime class type information.
Definition: CObject.h:47



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 04:08:57 UTC 2022