Main MRPT website > C++ reference
MRPT logo
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-2014, 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 
15 #include <mrpt/slam/CObservation.h>
17 
18 #include <mrpt/poses/CPoint2D.h>
19 #include <mrpt/poses/CPoint3D.h>
20 #include <mrpt/poses/CPose2D.h>
21 #include <mrpt/poses/CPose3D.h>
22 
23 #include <mrpt/utils/CObservable.h>
25 
26 namespace mrpt
27 {
28  namespace slam
29  {
30  using namespace mrpt::utils;
31 
32  class CObservation;
33  class CPointsMap;
34  class CSimplePointsMap;
35  class CSimpleMap;
36  class CSensoryFrame;
37 
38  /** Parameters for the determination of matchings between point clouds, etc. \sa CMetricMap::determineMatching2D, CMetricMap::determineMatching3D */
40  {
41  float maxDistForCorrespondence; //!< Maximum linear distance between two points to be paired (meters)
42  float maxAngularDistForCorrespondence; //!< Allowed "angular error" (in radians): this permits larger pairing threshold distances to more distant points.
43  bool onlyKeepTheClosest; //!< If set to true (default), only the closest correspondence will be returned. If false all are returned.
44  bool onlyUniqueRobust; //!< Additional consistency filter: "onlyKeepTheClosest" allows one correspondence for each "local map" point, but many of them may have as corresponding pair the same "global point", which this flag avoids.
45  size_t decimation_other_map_points; //!< (Default=1) Only consider 1 out of this number of points from the "other" map.
46  size_t offset_other_map_points; //!< Index of the first point in the "other" map to start checking for correspondences (Default=0)
47  mrpt::math::TPoint3D angularDistPivotPoint; //!< The point used to calculate angular distances: e.g. the coordinates of the sensor for a 2D laser scanner.
48 
49  /** Ctor: default values */
51  maxDistForCorrespondence(0.50f),
52  maxAngularDistForCorrespondence(.0f),
53  onlyKeepTheClosest(true),
54  onlyUniqueRobust(false),
55  decimation_other_map_points(1),
56  offset_other_map_points(0),
57  angularDistPivotPoint(0,0,0)
58  {}
59  };
60 
61  /** Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves \sa CMetricMap::determineMatching2D, CMetricMap::determineMatching3D */
63  {
64  float correspondencesRatio; //!< The ratio [0,1] of points in otherMap with at least one correspondence.
65  float sumSqrDist; //!< The sum of all matched points squared distances.If undesired, set to NULL, as default.
66 
67  TMatchingExtraResults() : correspondencesRatio(0),sumSqrDist(0)
68  {}
69  };
70 
72 
73  /** Declares a virtual base class for all metric maps storage classes.
74  * In this class virtual methods are provided to allow the insertion
75  * of any type of "CObservation" objects into the metric map, thus
76  * updating the map (doesn't matter if it is a 2D/3D grid, a point map, etc.).
77  *
78  * Observations don't include any information about the
79  * robot pose, just the raw observation and information about
80  * the sensor pose relative to the robot mobile base coordinates origin.
81  *
82  * Note that all metric maps implement this mrpt::utils::CObservable interface,
83  * emitting the following events:
84  * - mrpt::slam::mrptEventMetricMapClear: Upon call of the ::clear() method.
85  * - mrpt::slam::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).
86  *
87  * \sa CObservation, CSensoryFrame, CMultiMetricMap
88  * \ingroup mrpt_obs_grp
89  */
91  public mrpt::utils::CSerializable,
92  public mrpt::utils::CObservable
93  {
94  // This must be added to any CSerializable derived class:
96 
97  private:
98  /** Internal method called by clear() */
99  virtual void internal_clear() = 0;
100 
101  /** Hook for each time a "internal_insertObservation" returns "true"
102  * This is called automatically from insertObservation() when internal_insertObservation returns true.
103  */
105  {
106  // Default: do nothing
107  }
108 
109  /** Internal method called by insertObservation() */
110  virtual bool internal_insertObservation(
111  const CObservation *obs,
112  const CPose3D *robotPose = NULL ) = 0;
113 
114  public:
115  /** Erase all the contents of the map */
116  void clear();
117 
118  /** Returns true if the map is empty/no observation has been inserted.
119  */
120  virtual bool isEmpty() const = 0;
121 
122  /** Load the map contents from a CSimpleMap object, erasing all previous content of the map.
123  * This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as
124  * given by the "poses::CPosePDF" in the CSimpleMap object.
125  *
126  * \sa insertObservation, CSimpleMap
127  * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc...
128  */
129  void loadFromProbabilisticPosesAndObservations( const CSimpleMap &Map );
130 
131  /** Load the map contents from a CSimpleMap object, erasing all previous content of the map.
132  * This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as
133  * given by the "poses::CPosePDF" in the CSimpleMap object.
134  *
135  * \sa insertObservation, CSimpleMap
136  * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc...
137  */
138  inline void loadFromSimpleMap( const CSimpleMap &Map ) { loadFromProbabilisticPosesAndObservations(Map); }
139 
140  /** Insert the observation information into this map. This method must be implemented
141  * in derived classes.
142  * \param obs The observation
143  * \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.
144  *
145  * \sa CObservation::insertObservationInto
146  */
147  inline bool insertObservation(
148  const CObservation *obs,
149  const CPose3D *robotPose = NULL )
150  {
151  bool done = internal_insertObservation(obs,robotPose);
152  if (done)
153  {
154  OnPostSuccesfulInsertObs(obs);
155  publishEvent( mrptEventMetricMapInsert(this,obs,robotPose) );
156  }
157  return done;
158  }
159 
160  /** A wrapper for smart pointers, just calls the non-smart pointer version. */
161  inline bool insertObservationPtr(
162  const CObservationPtr &obs,
163  const CPose3D *robotPose = NULL )
164  {
165  MRPT_START
166  if (!obs.present()) { THROW_EXCEPTION("Trying to pass a null pointer."); }
167  return insertObservation(obs.pointer(),robotPose);
168  MRPT_END
169  }
170 
171  /** Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.
172  *
173  * \param takenFrom The robot's pose the observation is supposed to be taken from.
174  * \param obs The observation.
175  * \return This method returns a log-likelihood.
176  *
177  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
178  */
179  virtual double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ) = 0;
180 
181  /** Computes the log-likelihood of a given observation given an arbitrary robot 2D pose.
182  *
183  * \param takenFrom The robot's pose the observation is supposed to be taken from.
184  * \param obs The observation.
185  * \return This method returns a log-likelihood.
186  *
187  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
188  */
189  double computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom )
190  {
191  return computeObservationLikelihood(obs,CPose3D(takenFrom));
192  }
193 
194  /** 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).
195  * \param obs The observation.
196  * \sa computeObservationLikelihood
197  */
199  {
200  return true; // Unless implemented otherwise, assume we can always compute the likelihood.
201  }
202 
203  /** \overload */
205 
206  /** Returns the sum of the log-likelihoods of each individual observation within a mrpt::slam::CSensoryFrame.
207  *
208  * \param takenFrom The robot's pose the observation is supposed to be taken from.
209  * \param sf The set of observations in a CSensoryFrame.
210  * \return This method returns a log-likelihood.
211  * \sa canComputeObservationsLikelihood
212  */
213  double computeObservationsLikelihood( const CSensoryFrame &sf, const CPose2D &takenFrom );
214 
215  /** 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).
216  * \param sf The observations.
217  * \sa canComputeObservationLikelihood
218  */
219  bool canComputeObservationsLikelihood( const CSensoryFrame &sf );
220 
221  /** Constructor */
222  CMetricMap();
223 
224  /** Destructor */
225  virtual ~CMetricMap();
226 
227  /** Computes the matching between this and another 2D point map, which includes finding:
228  * - The set of points pairs in each map
229  * - The mean squared distance between corresponding pairs.
230  *
231  * The algorithm is:
232  * - For each point in "otherMap":
233  * - Transform the point according to otherMapPose
234  * - Search with a KD-TREE the closest correspondences in "this" map.
235  * - Add to the set of candidate matchings, if it passes all the thresholds in params.
236  *
237  * This method is the most time critical one into ICP-like algorithms.
238  *
239  * \param otherMap [IN] The other map to compute the matching with.
240  * \param otherMapPose [IN] The pose of the other map as seen from "this".
241  * \param params [IN] Parameters for the determination of pairings.
242  * \param correspondences [OUT] The detected matchings pairs.
243  * \param extraResults [OUT] Other results.
244  * \sa compute3DMatchingRatio
245  */
246  virtual void determineMatching2D(
247  const CMetricMap * otherMap,
248  const CPose2D & otherMapPose,
249  TMatchingPairList & correspondences,
250  const TMatchingParams & params,
251  TMatchingExtraResults & extraResults ) const
252  {
253  MRPT_START
254  THROW_EXCEPTION("Virtual method not implemented in derived class.")
255  MRPT_END
256  }
257 
258  /** Computes the matchings between this and another 3D points map - method used in 3D-ICP.
259  * This method finds the set of point pairs in each map.
260  *
261  * The method is the most time critical one into ICP-like algorithms.
262  *
263  * The algorithm is:
264  * - For each point in "otherMap":
265  * - Transform the point according to otherMapPose
266  * - Search with a KD-TREE the closest correspondences in "this" map.
267  * - Add to the set of candidate matchings, if it passes all the thresholds in params.
268  *
269  * \param otherMap [IN] The other map to compute the matching with.
270  * \param otherMapPose [IN] The pose of the other map as seen from "this".
271  * \param params [IN] Parameters for the determination of pairings.
272  * \param correspondences [OUT] The detected matchings pairs.
273  * \param extraResults [OUT] Other results.
274  * \sa compute3DMatchingRatio
275  */
276  virtual void determineMatching3D(
277  const CMetricMap * otherMap,
278  const CPose3D & otherMapPose,
279  TMatchingPairList & correspondences,
280  const TMatchingParams & params,
281  TMatchingExtraResults & extraResults ) const
282  {
283  MRPT_START
284  THROW_EXCEPTION("Virtual method not implemented in derived class.")
285  MRPT_END
286  }
287 
288 
289  /** DEPRECATED: Use alternative version determineMatching2D with struct parameters instead. */
290  MRPT_DEPRECATED_PRE("Deprecated")
291  void computeMatchingWith2D(
292  const CMetricMap *otherMap,
293  const CPose2D &otherMapPose,
294  float maxDistForCorrespondence,
295  float maxAngularDistForCorrespondence,
296  const CPose2D &angularDistPivotPoint,
297  TMatchingPairList &correspondences,
298  float &correspondencesRatio,
299  float *sumSqrDist = NULL,
300  bool onlyKeepTheClosest = true,
301  bool onlyUniqueRobust = false,
302  const size_t decimation_other_map_points = 1,
303  const size_t offset_other_map_points = 0 ) const MRPT_DEPRECATED_POST("Deprecated");
304 
305  /** DEPRECATED: Use alternative version determineMatching3D with struct parameters instead. */
306  MRPT_DEPRECATED_PRE("Deprecated")
307  void computeMatchingWith3D(
308  const CMetricMap *otherMap,
309  const CPose3D &otherMapPose,
310  float maxDistForCorrespondence,
311  float maxAngularDistForCorrespondence,
312  const CPoint3D &angularDistPivotPoint,
313  TMatchingPairList &correspondences,
314  float &correspondencesRatio,
315  float *sumSqrDist = NULL,
316  bool onlyKeepTheClosest = true,
317  bool onlyUniqueRobust = false,
318  const size_t decimation_other_map_points = 1,
319  const size_t offset_other_map_points = 0 ) const MRPT_DEPRECATED_POST("Deprecated");
320 
321 
322  /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
323  * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
324  * \param otherMap [IN] The other map to compute the matching with.
325  * \param otherMapPose [IN] The 6D pose of the other map as seen from "this".
326  * \param maxDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
327  * \param maxMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
328  *
329  * \return The matching ratio [0,1]
330  * \sa determineMatching2D
331  */
332  virtual float compute3DMatchingRatio(
333  const CMetricMap *otherMap,
334  const CPose3D &otherMapPose,
335  float maxDistForCorr = 0.10f,
336  float maxMahaDistForCorr = 2.0f
337  ) const
338  {
339  MRPT_START
340  THROW_EXCEPTION("Virtual method not implemented in derived class.")
341  MRPT_END
342  }
343 
344 
345  /** 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).
346  */
347  virtual void saveMetricMapRepresentationToFile(
348  const std::string &filNamePrefix
349  ) const = 0;
350 
351  /** Returns a 3D object representing the map.
352  */
353  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const = 0;
354 
355  /** When set to true (default=false), calling "getAs3DObject" will have no effects.
356  */
358 
359  /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
360  * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
361  */
363  {
364  // Default implementation: do nothing.
365  }
366 
367  /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
368  */
370  float x0,
371  float y0 ) const
372  {
373  MRPT_START
374  THROW_EXCEPTION("Virtual method not implemented in derived class.")
375  MRPT_END
376  }
377 
378 
379  /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
380  * Otherwise, return NULL
381  */
382  virtual const CSimplePointsMap * getAsSimplePointsMap() const { return NULL; }
383  virtual CSimplePointsMap * getAsSimplePointsMap() { return NULL; }
384 
385 
386  }; // End of class def.
387 
388  /** A list of metric maps (used in the mrpt::poses::CPosePDFParticles class):
389  */
390  typedef std::deque<CMetricMap*> TMetricMapList;
391 
392  } // End of namespace
393 } // End of namespace
394 
395 #endif
size_t decimation_other_map_points
(Default=1) Only consider 1 out of this number of points from the "other" map.
Definition: CMetricMap.h:45
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
void loadFromSimpleMap(const CSimpleMap &Map)
Load the map contents from a CSimpleMap object, erasing all previous content of the map...
Definition: CMetricMap.h:138
virtual void determineMatching2D(const CMetricMap *otherMap, const CPose2D &otherMapPose, TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const
Computes the matching between this and another 2D point map, which includes finding: ...
Definition: CMetricMap.h:246
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:32
float correspondencesRatio
The ratio [0,1] of points in otherMap with at least one correspondence.
Definition: CMetricMap.h:64
#define THROW_EXCEPTION(msg)
virtual void determineMatching3D(const CMetricMap *otherMap, const CPose3D &otherMapPose, TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const
Computes the matchings between this and another 3D points map - method used in 3D-ICP.
Definition: CMetricMap.h:276
bool insertObservation(const CObservation *obs, const CPose3D *robotPose=NULL)
Insert the observation information into this map.
Definition: CMetricMap.h:147
bool onlyKeepTheClosest
If set to true (default), only the closest correspondence will be returned. If false all are returned...
Definition: CMetricMap.h:43
bool canComputeObservationLikelihood(const CObservationPtr &obs)
Definition: CMetricMap.h:204
float maxDistForCorrespondence
Maximum linear distance between two points to be paired (meters)
Definition: CMetricMap.h:41
std::deque< CMetricMap * > TMetricMapList
A list of metric maps (used in the mrpt::poses::CPosePDFParticles class):
Definition: CMetricMap.h:390
#define DEFINE_VIRTUAL_SERIALIZABLE(class_name)
This declaration must be inserted in virtual CSerializable classes definition:
float sumSqrDist
The sum of all matched points squared distances.If undesired, set to NULL, as default.
Definition: CMetricMap.h:65
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:90
virtual void OnPostSuccesfulInsertObs(const CObservation *)
Hook for each time a "internal_insertObservation" returns "true" This is called automatically from in...
Definition: CMetricMap.h:104
#define MRPT_DEPRECATED_POST(_MSG)
#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...
virtual bool canComputeObservationLikelihood(const CObservation *obs)
Returns true if this map is able to compute a sensible likelihood function for this observation (i...
Definition: CMetricMap.h:198
bool insertObservationPtr(const CObservationPtr &obs, const CPose3D *robotPose=NULL)
A wrapper for smart pointers, just calls the non-smart pointer version.
Definition: CMetricMap.h:161
#define MRPT_END
CObservation * pointer()
Definition: CObservation.h:48
bool m_disableSaveAs3DObject
When set to true (default=false), calling "getAs3DObject" will have no effects.
Definition: CMetricMap.h:357
A list of TMatchingPair.
Definition: TMatchingPair.h:60
size_t offset_other_map_points
Index of the first point in the "other" map to start checking for correspondences (Default=0) ...
Definition: CMetricMap.h:46
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:61
float maxAngularDistForCorrespondence
Allowed "angular error" (in radians): this permits larger pairing threshold distances to more distant...
Definition: CMetricMap.h:42
Parameters for the determination of matchings between point clouds, etc.
Definition: CMetricMap.h:39
Event emitted by a metric up upon a succesful call to insertObservation()
A class used to store a 3D point.
Definition: CPoint3D.h:33
double computeObservationLikelihood(const CObservation *obs, const CPose2D &takenFrom)
Computes the log-likelihood of a given observation given an arbitrary robot 2D pose.
Definition: CMetricMap.h:189
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:33
#define MRPT_START
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:54
A class used to store a 2D pose.
Definition: CPose2D.h:35
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:71
virtual const 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 ...
Definition: CMetricMap.h:382
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
Definition: CMetricMap.h:62
mrpt::math::TPoint3D angularDistPivotPoint
The point used to calculate angular distances: e.g. the coordinates of the sensor for a 2D laser scan...
Definition: CMetricMap.h:47
bool onlyUniqueRobust
Additional consistency filter: "onlyKeepTheClosest" allows one correspondence for each "local map" po...
Definition: CMetricMap.h:44
virtual float squareDistanceToClosestCorrespondence(float x0, float y0) const
Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map...
Definition: CMetricMap.h:369
Lightweight 3D point.
#define MRPT_DEPRECATED_PRE(_MSG)
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans. ...
Inherit from this class for those objects capable of being observed by a CObserver class...
Definition: CObservable.h:31
virtual void auxParticleFilterCleanUp()
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
Definition: CMetricMap.h:362
TMatchingParams()
Ctor: default values.
Definition: CMetricMap.h:50
virtual CSimplePointsMap * getAsSimplePointsMap()
Definition: CMetricMap.h:383



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo