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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 3e8ecd670 Mon May 18 14:23:19 2020 +0200 at lun may 18 14:30:11 CEST 2020