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



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Tue Dec 8 09:49:21 UTC 2015