Main MRPT website > C++ reference
MRPT logo
CMetricMap.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CMetricMap_H
29 #define CMetricMap_H
30 
31 #include <mrpt/utils/CSerializable.h>
32 #include <mrpt/opengl/CSetOfObjects.h>
33 
34 #include <mrpt/slam/CObservation.h>
35 #include <mrpt/utils/TMatchingPair.h>
36 
37 #include <mrpt/poses/CPoint2D.h>
38 #include <mrpt/poses/CPoint3D.h>
39 #include <mrpt/poses/CPose2D.h>
40 #include <mrpt/poses/CPose3D.h>
41 
42 #include <mrpt/utils/CObservable.h>
43 #include <mrpt/slam/CMetricMapEvents.h>
44 
45 namespace mrpt
46 {
47  namespace slam
48  {
49  using namespace mrpt::utils;
50 
51  class CObservation;
52  class CPointsMap;
53  class CSimplePointsMap;
54  class CSimpleMap;
55  class CSensoryFrame;
56 
57  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CMetricMap, mrpt::utils::CSerializable, OBS_IMPEXP )
58 
59  /** Declares a virtual base class for all metric maps storage classes.
60  In this class virtual methods are provided to allow the insertion
61  of any type of "CObservation" objects into the metric map, thus
62  updating the map (doesn't matter if it is a 2D/3D grid or a points
63  map).
64  <b>IMPORTANT</b>: Observations doesn't include any information about the
65  robot pose beliefs, just the raw observation and information about
66  the sensor pose relative to the robot mobile base coordinates origin.
67  *
68  * Note that all metric maps implement this mrpt::utils::CObservable interface,
69  * emitting the following events:
70  * - mrpt::slam::mrptEventMetricMapClear: Upon call of the ::clear() method.
71  * - 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).
72  *
73  * \sa CObservation, CSensoryFrame, CMultiMetricMap
74  * \ingroup mrpt_obs_grp
75  */
76  class OBS_IMPEXP CMetricMap :
77  public mrpt::utils::CSerializable,
78  public mrpt::utils::CObservable
79  {
80  // This must be added to any CSerializable derived class:
81  DEFINE_VIRTUAL_SERIALIZABLE( CMetricMap )
82 
83  private:
84  /** Internal method called by clear() */
85  virtual void internal_clear() = 0;
86 
87  /** Hook for each time a "internal_insertObservation" returns "true"
88  * This is called automatically from insertObservation() when internal_insertObservation returns true.
89  */
90  virtual void OnPostSuccesfulInsertObs(const CObservation *)
91  {
92  // Default: do nothing
93  }
94 
95  /** Internal method called by insertObservation() */
96  virtual bool internal_insertObservation(
97  const CObservation *obs,
98  const CPose3D *robotPose = NULL ) = 0;
99 
100  public:
101  /** Erase all the contents of the map */
102  void clear();
103 
104  /** Returns true if the map is empty/no observation has been inserted.
105  */
106  virtual bool isEmpty() const = 0;
107 
108  /** Load the map contents from a CSimpleMap object, erasing all previous content of the map.
109  * This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as
110  * given by the "poses::CPosePDF" in the CSimpleMap object.
111  *
112  * \sa insertObservation, CSimpleMap
113  * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc...
114  */
115  void loadFromProbabilisticPosesAndObservations( const CSimpleMap &Map );
116 
117  /** Load the map contents from a CSimpleMap object, erasing all previous content of the map.
118  * This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as
119  * given by the "poses::CPosePDF" in the CSimpleMap object.
120  *
121  * \sa insertObservation, CSimpleMap
122  * \exception std::exception Some internal steps in invoked methods can raise exceptions on invalid parameters, etc...
123  */
124  inline void loadFromSimpleMap( const CSimpleMap &Map ) { loadFromProbabilisticPosesAndObservations(Map); }
125 
126  /** Insert the observation information into this map. This method must be implemented
127  * in derived classes.
128  * \param obs The observation
129  * \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.
130  *
131  * \sa CObservation::insertObservationInto
132  */
133  inline bool insertObservation(
134  const CObservation *obs,
135  const CPose3D *robotPose = NULL )
136  {
137  bool done = internal_insertObservation(obs,robotPose);
138  if (done)
139  {
140  OnPostSuccesfulInsertObs(obs);
141  publishEvent( mrptEventMetricMapInsert(this,obs,robotPose) );
142  }
143  return done;
144  }
145 
146  /** A wrapper for smart pointers, just calls the non-smart pointer version. */
147  inline bool insertObservationPtr(
148  const CObservationPtr &obs,
149  const CPose3D *robotPose = NULL )
150  {
151  MRPT_START
152  if (!obs.present()) { THROW_EXCEPTION("Trying to pass a null pointer."); }
153  return insertObservation(obs.pointer(),robotPose);
154  MRPT_END
155  }
156 
157  /** Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.
158  *
159  * \param takenFrom The robot's pose the observation is supposed to be taken from.
160  * \param obs The observation.
161  * \return This method returns a log-likelihood.
162  *
163  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
164  */
165  virtual double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom ) = 0;
166 
167  /** Computes the log-likelihood of a given observation given an arbitrary robot 2D pose.
168  *
169  * \param takenFrom The robot's pose the observation is supposed to be taken from.
170  * \param obs The observation.
171  * \return This method returns a log-likelihood.
172  *
173  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
174  */
175  double computeObservationLikelihood( const CObservation *obs, const CPose2D &takenFrom )
176  {
177  return computeObservationLikelihood(obs,CPose3D(takenFrom));
178  }
179 
180  /** 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).
181  * \param obs The observation.
182  * \sa computeObservationLikelihood
183  */
184  virtual bool canComputeObservationLikelihood( const CObservation *obs )
185  {
186  return true; // Unless implemented otherwise, we can always compute the likelihood.
187  }
188 
189  /** Returns the sum of the log-likelihoods of each individual observation within a mrpt::slam::CSensoryFrame.
190  *
191  * \param takenFrom The robot's pose the observation is supposed to be taken from.
192  * \param sf The set of observations in a CSensoryFrame.
193  * \return This method returns a log-likelihood.
194  * \sa canComputeObservationsLikelihood
195  */
196  double computeObservationsLikelihood( const CSensoryFrame &sf, const CPose2D &takenFrom );
197 
198  /** 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).
199  * \param sf The observations.
200  * \sa canComputeObservationLikelihood
201  */
202  bool canComputeObservationsLikelihood( const CSensoryFrame &sf );
203 
204  /** Constructor
205  */
206  CMetricMap();
207 
208  /** Destructor
209  */
210  virtual ~CMetricMap();
211 
212 #ifdef MRPT_BACKCOMPATIB_08X // For backward compatibility
213  typedef mrpt::utils::TMatchingPair TMatchingPair;
214  typedef mrpt::utils::TMatchingPairPtr TMatchingPairPtr;
215  typedef mrpt::utils::TMatchingPairList TMatchingPairList;
216 #endif
217 
218  /** Computes the matchings between this and another 2D points map.
219  This includes finding:
220  - The set of points pairs in each map
221  - The mean squared distance between corresponding pairs.
222  This method is the most time critical one into the ICP algorithm.
223 
224  * \param otherMap [IN] The other map to compute the matching with.
225  * \param otherMapPose [IN] The pose of the other map as seen from "this".
226  * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched.
227  * \param maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences.
228  * \param angularDistPivotPoint [IN] The point used to calculate distances from in both maps.
229  * \param correspondences [OUT] The detected matchings pairs.
230  * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
231  * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
232  * \param onlyKeepTheClosest [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
233  *
234  * \sa compute3DMatchingRatio
235  */
236  virtual void computeMatchingWith2D(
237  const CMetricMap *otherMap,
238  const CPose2D &otherMapPose,
239  float maxDistForCorrespondence,
240  float maxAngularDistForCorrespondence,
241  const CPose2D &angularDistPivotPoint,
242  TMatchingPairList &correspondences,
243  float &correspondencesRatio,
244  float *sumSqrDist = NULL,
245  bool onlyKeepTheClosest = true,
246  bool onlyUniqueRobust = false,
247  const size_t decimation_other_map_points = 1,
248  const size_t offset_other_map_points = 0 ) const
249  {
250  MRPT_START
251  THROW_EXCEPTION("Virtual method not implemented in derived class.")
252  MRPT_END
253  }
254 
255  /** Computes the matchings between this and another 3D points map - method used in 3D-ICP.
256  This method finds the set of point pairs in each map.
257 
258  The method is the most time critical one into the ICP algorithm.
259 
260  * \param otherMap [IN] The other map to compute the matching with.
261  * \param otherMapPose [IN] The pose of the other map as seen from "this".
262  * \param maxDistForCorrespondence [IN] Maximum 2D linear distance between two points to be matched.
263  * \param maxAngularDistForCorrespondence [IN] In radians: The aim is to allow larger distances to more distant correspondences.
264  * \param angularDistPivotPoint [IN] The point used to calculate distances from in both maps.
265  * \param correspondences [OUT] The detected matchings pairs.
266  * \param correspondencesRatio [OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
267  * \param sumSqrDist [OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
268  * \param onlyKeepTheClosest [IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
269  *
270  * \sa compute3DMatchingRatio
271  */
272  virtual void computeMatchingWith3D(
273  const CMetricMap *otherMap,
274  const CPose3D &otherMapPose,
275  float maxDistForCorrespondence,
276  float maxAngularDistForCorrespondence,
277  const CPoint3D &angularDistPivotPoint,
278  TMatchingPairList &correspondences,
279  float &correspondencesRatio,
280  float *sumSqrDist = NULL,
281  bool onlyKeepTheClosest = true,
282  bool onlyUniqueRobust = false,
283  const size_t decimation_other_map_points = 1,
284  const size_t offset_other_map_points = 0 ) const
285  {
286  MRPT_START
287  THROW_EXCEPTION("Virtual method not implemented in derived class.")
288  MRPT_END
289  }
290 
291 
292  /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
293  * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
294  * \param otherMap [IN] The other map to compute the matching with.
295  * \param otherMapPose [IN] The 6D pose of the other map as seen from "this".
296  * \param minDistForCorr [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence.
297  * \param minMahaDistForCorr [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence.
298  *
299  * \return The matching ratio [0,1]
300  * \sa computeMatchingWith2D
301  */
302  virtual float compute3DMatchingRatio(
303  const CMetricMap *otherMap,
304  const CPose3D &otherMapPose,
305  float minDistForCorr = 0.10f,
306  float minMahaDistForCorr = 2.0f
307  ) const = 0;
308 
309  /** 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).
310  */
311  virtual void saveMetricMapRepresentationToFile(
312  const std::string &filNamePrefix
313  ) const = 0;
314 
315  /** Returns a 3D object representing the map.
316  */
317  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const = 0;
318 
319  /** When set to true (default=false), calling "getAs3DObject" will have no effects.
320  */
321  bool m_disableSaveAs3DObject;
322 
323  /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
324  * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
325  */
326  virtual void auxParticleFilterCleanUp()
327  {
328  // Default implementation: do nothing.
329  }
330 
331  /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
332  */
333  virtual float squareDistanceToClosestCorrespondence(
334  const float &x0,
335  const float &y0 ) const
336  {
337  MRPT_START
338  THROW_EXCEPTION("Virtual method not implemented in derived class.")
339  MRPT_END
340  }
341 
342 
343  /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
344  * Otherwise, return NULL
345  */
346  virtual const CSimplePointsMap * getAsSimplePointsMap() const { return NULL; }
347  virtual CSimplePointsMap * getAsSimplePointsMap() { return NULL; }
348 
349 
350  }; // End of class def.
351 
352  /** A list of metric maps (used in the mrpt::poses::CPosePDFParticles class):
353  */
354  typedef std::deque<CMetricMap*> TMetricMapList;
355 
356  } // End of namespace
357 } // End of namespace
358 
359 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013