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>
29#include <mrpt/obs/obs_frwds.h>
30
32
33namespace mrpt
34{
35namespace 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 */
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 */
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
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
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.
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...
EIGEN_STRONG_INLINE iterator begin()
EIGEN_STRONG_INLINE iterator end()
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition mrpt_macros.h:28
struct OBS_IMPEXP CMetricMapPtr
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
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.8 for MRPT 1.4.0 SVN: at Wed Dec 6 15:06:50 UTC 2023