Main MRPT website > C++ reference for MRPT 1.5.6
maps/CMetricMap.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-2017, 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 CMetricMap_H
10 #define CMetricMap_H
11 
14 #include <mrpt/utils/CObservable.h>
15 #include <mrpt/math/math_frwds.h>
21 #include <mrpt/obs/obs_frwds.h>
22 #include <mrpt/obs/link_pragmas.h>
23 #include <deque>
24 
25 namespace mrpt
26 {
27  namespace maps
28  {
30 
31  /** Declares a virtual base class for all metric maps storage classes.
32  * In this class virtual methods are provided to allow the insertion
33  * of any type of "CObservation" objects into the metric map, thus
34  * updating the map (doesn't matter if it is a 2D/3D grid, a point map, etc.).
35  *
36  * Observations don't include any information about the
37  * robot pose, just the raw observation and information about
38  * the sensor pose relative to the robot mobile base coordinates origin.
39  *
40  * Note that all metric maps implement this mrpt::utils::CObservable interface,
41  * emitting the following events:
42  * - mrpt::obs::mrptEventMetricMapClear: Upon call of the ::clear() method.
43  * - mrpt::obs::mrptEventMetricMapInsert: Upon insertion of an observation that effectively modifies the map (e.g. inserting an image into a grid map will NOT raise an event, inserting a laser scan will).
44  *
45  * To check what observations are supported by each metric map, see: \ref maps_observations
46  *
47  * \note All derived class must implement a static class factory `<metric_map_class>::MapDefinition()` that builds a default TMetricMapInitializer [New in MRPT 1.3.0]
48  *
49  * \sa CObservation, CSensoryFrame, CMultiMetricMap
50  * \ingroup mrpt_obs_grp
51  */
53  public mrpt::utils::CSerializable,
54  public mrpt::utils::CObservable
55  {
56  // This must be added to any CSerializable derived class:
58 
59  private:
60  /** Internal method called by clear() */
61  virtual void internal_clear() = 0;
62 
63  /** Internal method called by insertObservation() */
64  virtual bool internal_insertObservation(
65  const mrpt::obs::CObservation *obs,
66  const mrpt::poses::CPose3D *robotPose = NULL ) = 0;
67 
68  /** Internal method called by computeObservationLikelihood() */
69  virtual double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) = 0;
70  /** Internal method called by canComputeObservationLikelihood() */
72  {
73  MRPT_UNUSED_PARAM(obs);
74  return true; // Unless implemented otherwise, assume we can always compute the likelihood.
75  }
76 
77  /** Hook for each time a "internal_insertObservation" returns "true"
78  * This is called automatically from insertObservation() when internal_insertObservation returns true. */
79  virtual void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation *) { /* Default: do nothing */ }
80 
81  public:
82  /** Erase all the contents of the map */
83  void clear();
84 
85  /** Returns true if the map is empty/no observation has been inserted.
86  */
87  virtual bool isEmpty() const = 0;
88 
89  /** Load the map contents from a CSimpleMap object, erasing all previous content of the map.
90  * This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as
91  * given by the "poses::CPosePDF" in the CSimpleMap object.
92  *
93  * \sa insertObservation, CSimpleMap
94  * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc...
95  */
96  void loadFromProbabilisticPosesAndObservations( const mrpt::maps::CSimpleMap &Map );
97 
98  /** Load the map contents from a CSimpleMap object, erasing all previous content of the map.
99  * This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as
100  * given by the "poses::CPosePDF" in the CSimpleMap object.
101  *
102  * \sa insertObservation, CSimpleMap
103  * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc...
104  */
105  inline void loadFromSimpleMap( const mrpt::maps::CSimpleMap &Map ) { loadFromProbabilisticPosesAndObservations(Map); }
106 
107  /** Insert the observation information into this map. This method must be implemented
108  * in derived classes. See: \ref maps_observations
109  * \param obs The observation
110  * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use the origin.
111  *
112  * \sa CObservation::insertObservationInto
113  */
114  bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL );
115 
116  /** A wrapper for smart pointers, just calls the non-smart pointer version. See: \ref maps_observations */
117  bool insertObservationPtr(const mrpt::obs::CObservationPtr &obs, const mrpt::poses::CPose3D *robotPose = NULL );
118 
119  /** Computes the log-likelihood of a given observation given an arbitrary robot 3D pose. See: \ref maps_observations
120  *
121  * \param takenFrom The robot's pose the observation is supposed to be taken from.
122  * \param obs The observation.
123  * \return This method returns a log-likelihood.
124  *
125  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
126  */
127  double computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom );
128 
129  /** \overload */
130  double computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom );
131 
132  /** Returns true if this map is able to compute a sensible likelihood function for this observation (i.e. an occupancy grid map cannot with an image). See: \ref maps_observations
133  * \param obs The observation.
134  * \sa computeObservationLikelihood, genericMapParams.enableObservationLikelihood
135  */
136  virtual bool canComputeObservationLikelihood( const mrpt::obs::CObservation *obs ) const;
137 
138  /** \overload */
139  bool canComputeObservationLikelihood( const mrpt::obs::CObservationPtr &obs ) const;
140 
141  /** Returns the sum of the log-likelihoods of each individual observation within a mrpt::obs::CSensoryFrame. See: \ref maps_observations
142  *
143  * \param takenFrom The robot's pose the observation is supposed to be taken from.
144  * \param sf The set of observations in a CSensoryFrame.
145  * \return This method returns a log-likelihood.
146  * \sa canComputeObservationsLikelihood
147  */
148  double computeObservationsLikelihood( const mrpt::obs::CSensoryFrame &sf, const mrpt::poses::CPose2D &takenFrom );
149 
150  /** Returns true if this map is able to compute a sensible likelihood function for this observation (i.e. an occupancy grid map cannot with an image). See: \ref maps_observations
151  * \param sf The observations.
152  * \sa canComputeObservationLikelihood
153  */
154  bool canComputeObservationsLikelihood( const mrpt::obs::CSensoryFrame &sf ) const;
155 
156  /** Constructor */
157  CMetricMap();
158 
159  /** Computes the matching between this and another 2D point map, which includes finding:
160  * - The set of points pairs in each map
161  * - The mean squared distance between corresponding pairs.
162  *
163  * The algorithm is:
164  * - For each point in "otherMap":
165  * - Transform the point according to otherMapPose
166  * - Search with a KD-TREE the closest correspondences in "this" map.
167  * - Add to the set of candidate matchings, if it passes all the thresholds in params.
168  *
169  * This method is the most time critical one into ICP-like algorithms.
170  *
171  * \param otherMap [IN] The other map to compute the matching with.
172  * \param otherMapPose [IN] The pose of the other map as seen from "this".
173  * \param params [IN] Parameters for the determination of pairings.
174  * \param correspondences [OUT] The detected matchings pairs.
175  * \param extraResults [OUT] Other results.
176  * \sa compute3DMatchingRatio
177  */
178  virtual void determineMatching2D(
179  const mrpt::maps::CMetricMap * otherMap,
180  const mrpt::poses::CPose2D & otherMapPose,
181  mrpt::utils::TMatchingPairList & correspondences,
182  const TMatchingParams & params,
183  TMatchingExtraResults & extraResults ) const;
184 
185  /** Computes the matchings between this and another 3D points map - method used in 3D-ICP.
186  * This method finds the set of point pairs in each map.
187  *
188  * The method is the most time critical one into ICP-like algorithms.
189  *
190  * The algorithm is:
191  * - For each point in "otherMap":
192  * - Transform the point according to otherMapPose
193  * - Search with a KD-TREE the closest correspondences in "this" map.
194  * - Add to the set of candidate matchings, if it passes all the thresholds in params.
195  *
196  * \param otherMap [IN] The other map to compute the matching with.
197  * \param otherMapPose [IN] The pose of the other map as seen from "this".
198  * \param params [IN] Parameters for the determination of pairings.
199  * \param correspondences [OUT] The detected matchings pairs.
200  * \param extraResults [OUT] Other results.
201  * \sa compute3DMatchingRatio
202  */
203  virtual void determineMatching3D(
204  const mrpt::maps::CMetricMap * otherMap,
205  const mrpt::poses::CPose3D & otherMapPose,
206  mrpt::utils::TMatchingPairList & correspondences,
207  const TMatchingParams & params,
208  TMatchingExtraResults & extraResults ) const;
209 
210  /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
211  * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
212  * \param otherMap [IN] The other map to compute the matching with.
213  * \param otherMapPose [IN] The 6D pose of the other map as seen from "this".
214  * \param params [IN] Matching parameters
215  * \return The matching ratio [0,1]
216  * \sa determineMatching2D
217  */
218  virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const;
219 
220  /** This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface). */
221  virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const = 0;
222 
223  /** Returns a 3D object representing the map.
224  * \sa genericMapParams, TMapGenericParams::enableSaveAs3DObject */
225  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const = 0;
226 
227  TMapGenericParams genericMapParams; //!< Common params to all maps
228 
229  /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
230  * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
231  */
232  virtual void auxParticleFilterCleanUp() { /* Default implementation: do nothing. */ }
233 
234  /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map. */
235  virtual float squareDistanceToClosestCorrespondence(float x0,float y0 ) const;
236 
237  /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
238  * Otherwise, return NULL
239  */
240  virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const { return NULL; }
242 
243  }; // End of class def.
245 
246  /** A list of metric maps (used in the mrpt::poses::CPosePDFParticles class):
247  */
248  typedef std::deque<CMetricMap*> TMetricMapList;
249 
250  } // End of namespace
251 } // End of namespace
252 
253 #endif
Parameters for CMetricMap::compute3DMatchingRatio()
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
virtual void auxParticleFilterCleanUp()
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
#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.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
virtual bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation *obs) const
Internal method called by canComputeObservationLikelihood()
std::deque< CMetricMap * > TMetricMapList
A list of metric maps (used in the mrpt::poses::CPosePDFParticles class):
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
GLsizei const GLchar ** string
Definition: glext.h:3919
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot&#39;s observation.
Common params to all maps derived from mrpt::maps::CMetricMap.
virtual mrpt::maps::CSimplePointsMap * getAsSimplePointsMap()
virtual void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation *)
Hook for each time a "internal_insertObservation" returns "true" This is called automatically from in...
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const
If the map is a simple points map or it&#39;s a multi-metric map that contains EXACTLY one simple points ...
Parameters for the determination of matchings between point clouds, etc.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
void loadFromSimpleMap(const mrpt::maps::CSimpleMap &Map)
Load the map contents from a CSimpleMap object, erasing all previous content of the map...
GLenum const GLfloat * params
Definition: glext.h:3514



Page generated by Doxygen 1.8.14 for MRPT 1.5.6 Git: 4c65e8431 Tue Apr 24 08:18:17 2018 +0200 at lun oct 28 01:35:26 CET 2019