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.
A list of TMatchingPair.
Definition: TMatchingPair.h:66
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&#39;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 class used to store a 2D pose.
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
unsigned int m_ID
An auxiliary variable that can be used freely by the users (this will be copied to other maps using t...
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.11 for MRPT 1.3.2 SVN: at Wed May 25 02:34:21 UTC 2016