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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019