Main MRPT website > C++ reference for MRPT 1.4.0
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-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 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  * Checkout each derived class documentation to learn which mrpt::obs::CObservation-derived classes are supported by CMetricMap::insertObservation()
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() */
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  */
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.
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. */
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.
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  */
128 
129  /** \overload */
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).
133  * \param obs The observation.
134  * \sa computeObservationLikelihood, genericMapParams.enableObservationLikelihood
135  */
137 
138  /** \overload */
139  bool canComputeObservationLikelihood( const mrpt::obs::CObservationPtr &obs );
140 
141  /** Returns the sum of the log-likelihoods of each individual observation within a mrpt::obs::CSensoryFrame.
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  */
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).
151  * \param sf The observations.
152  * \sa canComputeObservationLikelihood
153  */
155 
156  /** Constructor */
158 
159  /** Destructor */
160  virtual ~CMetricMap();
161 
162  /** Computes the matching between this and another 2D point map, which includes finding:
163  * - The set of points pairs in each map
164  * - The mean squared distance between corresponding pairs.
165  *
166  * The algorithm is:
167  * - For each point in "otherMap":
168  * - Transform the point according to otherMapPose
169  * - Search with a KD-TREE the closest correspondences in "this" map.
170  * - Add to the set of candidate matchings, if it passes all the thresholds in params.
171  *
172  * This method is the most time critical one into ICP-like algorithms.
173  *
174  * \param otherMap [IN] The other map to compute the matching with.
175  * \param otherMapPose [IN] The pose of the other map as seen from "this".
176  * \param params [IN] Parameters for the determination of pairings.
177  * \param correspondences [OUT] The detected matchings pairs.
178  * \param extraResults [OUT] Other results.
179  * \sa compute3DMatchingRatio
180  */
181  virtual void determineMatching2D(
182  const mrpt::maps::CMetricMap * otherMap,
183  const mrpt::poses::CPose2D & otherMapPose,
184  mrpt::utils::TMatchingPairList & correspondences,
185  const TMatchingParams & params,
186  TMatchingExtraResults & extraResults ) const;
187 
188  /** Computes the matchings between this and another 3D points map - method used in 3D-ICP.
189  * This method finds the set of point pairs in each map.
190  *
191  * The method is the most time critical one into ICP-like algorithms.
192  *
193  * The algorithm is:
194  * - For each point in "otherMap":
195  * - Transform the point according to otherMapPose
196  * - Search with a KD-TREE the closest correspondences in "this" map.
197  * - Add to the set of candidate matchings, if it passes all the thresholds in params.
198  *
199  * \param otherMap [IN] The other map to compute the matching with.
200  * \param otherMapPose [IN] The pose of the other map as seen from "this".
201  * \param params [IN] Parameters for the determination of pairings.
202  * \param correspondences [OUT] The detected matchings pairs.
203  * \param extraResults [OUT] Other results.
204  * \sa compute3DMatchingRatio
205  */
206  virtual void determineMatching3D(
207  const mrpt::maps::CMetricMap * otherMap,
208  const mrpt::poses::CPose3D & otherMapPose,
209  mrpt::utils::TMatchingPairList & correspondences,
210  const TMatchingParams & params,
211  TMatchingExtraResults & extraResults ) const;
212 
213  /** Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose"
214  * In the case of a multi-metric map, this returns the average between the maps. This method always return 0 for grid maps.
215  * \param otherMap [IN] The other map to compute the matching with.
216  * \param otherMapPose [IN] The 6D pose of the other map as seen from "this".
217  * \param params [IN] Matching parameters
218  * \return The matching ratio [0,1]
219  * \sa determineMatching2D
220  */
221  virtual float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const;
222 
223  /** 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). */
224  virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const = 0;
225 
226  /** Returns a 3D object representing the map.
227  * \sa genericMapParams, TMapGenericParams::enableSaveAs3DObject */
228  virtual void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const = 0;
229 
230  TMapGenericParams genericMapParams; //!< Common params to all maps
231 
232  /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
233  * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
234  */
235  virtual void auxParticleFilterCleanUp() { /* Default implementation: do nothing. */ }
236 
237  /** Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map. */
238  virtual float squareDistanceToClosestCorrespondence(float x0,float y0 ) const;
239 
240  /** If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
241  * Otherwise, return NULL
242  */
243  virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const { return NULL; }
245 
246  }; // End of class def.
248 
249  /** A list of metric maps (used in the mrpt::poses::CPosePDFParticles class):
250  */
251  typedef std::deque<CMetricMap*> TMetricMapList;
252 
253  } // End of namespace
254 } // End of namespace
255 
256 #endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#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...
Declares a virtual base class for all metric maps storage classes.
double computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose2D &takenFrom)
This is an overloaded member function, provided for convenience. It differs from the above function o...
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...
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,...
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.
TMapGenericParams genericMapParams
Common params to all maps.
bool insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)
Insert the observation information into this map.
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.
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const =0
Returns a 3D object representing the map.
virtual void internal_clear()=0
Internal method called by clear()
void loadFromProbabilisticPosesAndObservations(const mrpt::maps::CSimpleMap &Map)
Load the map contents from a CSimpleMap object, erasing all previous content of the map.
bool canComputeObservationsLikelihood(const mrpt::obs::CSensoryFrame &sf)
Returns true if this map is able to compute a sensible likelihood function for this observation (i....
virtual void determineMatching3D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const
Computes the matchings between this and another 3D points map - method used in 3D-ICP.
virtual void auxParticleFilterCleanUp()
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
virtual ~CMetricMap()
Destructor.
void loadFromSimpleMap(const mrpt::maps::CSimpleMap &Map)
Load the map contents from a CSimpleMap object, erasing all previous content of the map.
bool insertObservationPtr(const mrpt::obs::CObservationPtr &obs, const mrpt::poses::CPose3D *robotPose=NULL)
A wrapper for smart pointers, just calls the non-smart pointer version.
virtual double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom)=0
Internal method called by computeObservationLikelihood()
virtual void OnPostSuccesfulInsertObs(const mrpt::obs::CObservation *)
Hook for each time a "internal_insertObservation" returns "true" This is called automatically from in...
virtual bool canComputeObservationLikelihood(const mrpt::obs::CObservation *obs)
Returns true if this map is able to compute a sensible likelihood function for this observation (i....
bool canComputeObservationLikelihood(const mrpt::obs::CObservationPtr &obs)
This is an overloaded member function, provided for convenience. It differs from the above function o...
virtual bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL)=0
Internal method called by insertObservation()
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::utils::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const
Computes the matching between this and another 2D point map, which includes finding:
virtual const mrpt::maps::CSimplePointsMap * getAsSimplePointsMap() const
If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points ...
virtual bool isEmpty() const =0
Returns true if the map is empty/no observation has been inserted.
CMetricMap()
Constructor.
void clear()
Erase all the contents of the map.
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const =0
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >,...
virtual bool internal_canComputeObservationLikelihood(const mrpt::obs::CObservation *obs)
Internal method called by canComputeObservationLikelihood()
virtual mrpt::maps::CSimplePointsMap * getAsSimplePointsMap()
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
A cloud of points in 2D or 3D, which can be built from a sequence of laser scans.
Common params to all maps derived from mrpt::maps::CMetricMap
Declares a class that represents any robot's observation.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
A class used to store a 2D pose.
Definition: CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
The virtual base class which provides a unified interface for all persistent objects in MRPT.
Definition: CSerializable.h:40
A list of TMatchingPair.
Definition: TMatchingPair.h:67
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
Definition: mrpt_macros.h:290
std::deque< CMetricMap * > TMetricMapList
A list of metric maps (used in the mrpt::poses::CPosePDFParticles class):
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
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()



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Sun Mar 7 18:43:46 UTC 2021