Main MRPT website > C++ reference for MRPT 1.9.9
CBeaconMap.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 CBeaconMap_H
10 #define CBeaconMap_H
11 
12 #include <mrpt/maps/CMetricMap.h>
13 #include <mrpt/maps/CBeacon.h>
15 #include <mrpt/math/CMatrix.h>
17 #include <mrpt/obs/obs_frwds.h>
18 
19 namespace mrpt
20 {
21 namespace maps
22 {
23 /** A class for storing a map of 3D probabilistic beacons, using a Montecarlo,
24  *Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
25  * <br>
26  * The individual beacons are defined as mrpt::maps::CBeacon objects.
27  * <br>
28  * When invoking CBeaconMap::insertObservation(), landmarks will be extracted
29  *and fused into the map.
30  * The only currently supported observation type is
31  *mrpt::obs::CObservationBeaconRanges.
32  * See insertionOptions and likelihoodOptions for parameters used when
33  *creating and fusing beacon landmarks.
34  * <br>
35  * Use "TInsertionOptions::insertAsMonteCarlo" to select between 2 different
36  *behaviors:
37  * - Initial PDF of beacons: MonteCarlo, after convergence, pass to
38  *Gaussians; or
39  * - Initial PDF of beacons: SOG, after convergence, a single Gaussian.
40  *
41  * Refer to the papers: []
42  *
43  * \ingroup mrpt_maps_grp
44  * \sa CMetricMap
45  */
47 {
49 
50  public:
51  using TSequenceBeacons = std::deque<CBeacon>;
54 
55  protected:
56  /** The individual beacons */
58 
59  // See docs in base class
60  virtual void internal_clear() override;
61  virtual bool internal_insertObservation(
62  const mrpt::obs::CObservation* obs,
63  const mrpt::poses::CPose3D* robotPose = nullptr) override;
65  const mrpt::obs::CObservation* obs,
66  const mrpt::poses::CPose3D& takenFrom) override;
67 
68  public:
69  /** Constructor */
70  CBeaconMap();
71 
72  /** Resize the number of SOG modes */
73  void resize(const size_t N);
74 
75  /** Access to individual beacons */
76  const CBeacon& operator[](size_t i) const
77  {
78  ASSERT_(i < m_beacons.size());
79  return m_beacons[i];
80  }
81  /** Access to individual beacons */
82  const CBeacon& get(size_t i) const
83  {
84  ASSERT_(i < m_beacons.size());
85  return m_beacons[i];
86  }
87  /** Access to individual beacons */
88  CBeacon& operator[](size_t i)
89  {
90  ASSERT_(i < m_beacons.size());
91  return m_beacons[i];
92  }
93  /** Access to individual beacons */
94  CBeacon& get(size_t i)
95  {
96  ASSERT_(i < m_beacons.size());
97  return m_beacons[i];
98  }
99 
100  iterator begin() { return m_beacons.begin(); }
101  const_iterator begin() const { return m_beacons.begin(); }
102  iterator end() { return m_beacons.end(); }
103  const_iterator end() const { return m_beacons.end(); }
104  /** Inserts a copy of the given mode into the SOG */
105  void push_back(const CBeacon& m) { m_beacons.push_back(m); }
106  // See docs in base class
108  const mrpt::maps::CMetricMap* otherMap,
109  const mrpt::poses::CPose3D& otherMapPose,
110  const TMatchingRatioParams& params) const override;
111 
112  /** With this struct options are provided to the likelihood computations */
114  {
115  public:
116  void loadFromConfigFile(
118  const std::string& section) override; // See base docs
119  void dumpToTextStream(
120  std::ostream& out) const override; // See base docs
121 
122  /** The standard deviation used for Beacon ranges likelihood
123  * (default=0.08m).
124  */
125  double rangeStd = {0.08};
127 
128  /** This struct contains data for choosing the method by which new beacons
129  * are inserted in the map.
130  */
132  {
133  public:
134  /** Initilization of default parameters */
135  void loadFromConfigFile(
137  const std::string& section) override; // See base docs
138  void dumpToTextStream(
139  std::ostream& out) const override; // See base docs
140 
141  /** Insert a new beacon as a set of montecarlo samples (default=true),
142  * or, if false, as a sum of gaussians (see mrpt::maps::CBeacon).
143  * \sa MC_performResampling
144  */
145  bool insertAsMonteCarlo{true};
146 
147  /** Minimum and maximum elevation angles (in degrees) for inserting new
148  * beacons at the first observation: the default values (both 0), makes
149  * the beacons to be in the same horizontal plane that the sensors, that
150  * is, 2D SLAM - the min/max values are -90/90.
151  */
153 
154  /** Number of particles per meter of range, i.e. per meter of the
155  * "radius of the ring".
156  */
157  unsigned int MC_numSamplesPerMeter{1000};
158 
159  /** The threshold for the maximum std (X,Y,and Z) before colapsing the
160  * particles into a Gaussian PDF (default=0.4).
161  */
162  float MC_maxStdToGauss = {0.4f};
163 
164  /** Threshold for the maximum difference from the maximun (log) weight
165  * in the set of samples for erasing a given sample (default=5).
166  */
168 
169  /** If set to false (default), the samples will be generated the first
170  * time a beacon is observed, and their weights just updated
171  * subsequently - if set to "true", fewer samples will be required since
172  * the particles will be resamples when necessary, and a small "noise"
173  * will be added to avoid depletion.
174  */
175  bool MC_performResampling{false};
176 
177  /** The std.dev. of the Gaussian noise to be added to each sample after
178  * resampling, only if MC_performResampling=true.
179  */
181 
182  /** Threshold for the maximum difference from the maximun (log) weight
183  * in the SOG for erasing a given mode (default=20).
184  */
186 
187  /** A parameter for initializing 2D/3D SOGs
188  */
190 
191  /** Constant used to compute the std. dev. int the tangent direction
192  * when creating the Gaussians.
193  */
196 
197  /** Save to a MATLAB script which displays 3D error ellipses for the map.
198  * \param file The name of the file to save the script to.
199  * \param style The MATLAB-like string for the style of the lines (see
200  *'help plot' in MATLAB for posibilities)
201  * \param stdCount The ellipsoids will be drawn from the center to a given
202  *confidence interval in [0,1], e.g. 2 sigmas=0.95 (default is 2std = 0.95
203  *confidence intervals)
204  *
205  * \return Returns false if any error occured, true elsewere.
206  */
208  const std::string& file, const char* style = "b",
209  float confInterval = 0.95f) const;
210 
211  /** Returns the stored landmarks count.
212  */
213  size_t size() const;
214 
215  // See docs in base class
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 override;
222 
223  /** Perform a search for correspondences between "this" and another
224  * lansmarks map:
225  * Firsly, the landmarks' descriptor is used to find correspondences, then
226  * inconsistent ones removed by
227  * looking at their 3D poses.
228  * \param otherMap [IN] The other map.
229  * \param correspondences [OUT] The matched pairs between maps.
230  * \param correspondencesRatio [OUT] This is NumberOfMatchings /
231  * NumberOfLandmarksInTheAnotherMap
232  * \param otherCorrespondences [OUT] Will be returned with a vector
233  * containing "true" for the indexes of the other map's landmarks with a
234  * correspondence.
235  */
237  const mrpt::maps::CBeaconMap* otherMap,
238  mrpt::tfest::TMatchingPairList& correspondences,
239  float& correspondencesRatio,
240  std::vector<bool>& otherCorrespondences) const;
241 
242  /** Changes the reference system of the map to a given 3D pose.
243  */
245 
246  /** Changes the reference system of the map "otherMap" and save the result
247  * in "this" map.
248  */
250  const mrpt::poses::CPose3D& newOrg,
251  const mrpt::maps::CBeaconMap* otherMap);
252 
253  /** Returns true if the map is empty/no observation has been inserted.
254  */
255  bool isEmpty() const override;
256 
257  /** Simulates a reading toward each of the beacons in the landmarks map, if
258  * any.
259  * \param in_robotPose This robot pose is used to simulate the ranges to
260  * each beacon.
261  * \param in_sensorLocationOnRobot The 3D position of the sensor on the
262  * robot
263  * \param out_Observations The results will be stored here. NOTICE that the
264  * fields
265  * "CObservationBeaconRanges::minSensorDistance","CObservationBeaconRanges::maxSensorDistance"
266  * and "CObservationBeaconRanges::stdError" MUST BE FILLED OUT before
267  * calling this function.
268  * An observation will be generated for each beacon in the map, but notice
269  * that some of them may be missed if out of the sensor maximum range.
270  */
272  const mrpt::poses::CPose3D& in_robotPose,
273  const mrpt::poses::CPoint3D& in_sensorLocationOnRobot,
274  mrpt::obs::CObservationBeaconRanges& out_Observations) const;
275 
276  /** This virtual method saves the map to a file "filNamePrefix"+<
277  *some_file_extension >, as an image or in any other applicable way (Notice
278  *that other methods to save the map may be implemented in classes
279  *implementing this virtual interface).
280  * In the case of this class, these files are generated:
281  * - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks
282  *as
283  *3D ellipses.
284  * - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane
285  *grid"
286  *and the set of ellipsoids in 3D.
287  * - "filNamePrefix"+"_covs.m": A textual representation (see
288  *saveToTextFile)
289  */
291  const std::string& filNamePrefix) const override;
292 
293  /** Save a text file with a row per beacon, containing this 11 elements:
294  * - X Y Z: Mean values
295  * - VX VY VZ: Variances of each dimension (C11, C22, C33)
296  * - DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
297  * - C12, C13, C23: Cross covariances
298  */
299  void saveToTextFile(const std::string& fil) const;
300 
301  /** Returns a 3D object representing the map. */
302  void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
303 
304  /** Returns a pointer to the beacon with the given ID, or nullptr if it does
305  * not exist. */
306  const CBeacon* getBeaconByID(CBeacon::TBeaconID id) const;
307  /** Returns a pointer to the beacon with the given ID, or nullptr if it does
308  * not exist. */
310 
312  /** Observations insertion options */
313  mrpt::maps::CBeaconMap::TInsertionOptions insertionOpts;
314  /** Probabilistic observation likelihood options */
315  mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOpts;
317 
318 }; // End of class def.
319 
320 } // End of namespace
321 } // End of namespace
322 
323 #endif
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
Scalar * iterator
Definition: eigen_plugins.h:26
Parameters for CMetricMap::compute3DMatchingRatio()
const_iterator end() const
Definition: CBeaconMap.h:103
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
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.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
This struct contains data for choosing the method by which new beacons are inserted in the map...
Definition: CBeaconMap.h:131
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
With this struct options are provided to the likelihood computations.
Definition: CBeaconMap.h:113
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
virtual void internal_clear() override
Internal method called by clear()
Definition: CBeaconMap.cpp:94
bool MC_performResampling
If set to false (default), the samples will be generated the first time a beacon is observed...
Definition: CBeaconMap.h:175
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CBeaconMap.cpp:962
double maxElevation_deg
Minimum and maximum elevation angles (in degrees) for inserting new beacons at the first observation:...
Definition: CBeaconMap.h:152
TSequenceBeacons m_beacons
The individual beacons.
Definition: CBeaconMap.h:57
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This class allows loading and storing values and vectors of different types from a configuration text...
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
Definition: CBeaconMap.cpp:823
int64_t TBeaconID
The type for the IDs of landmarks.
Definition: CBeacon.h:45
Additional results from the determination of matchings between point clouds, etc., apart from the pairings themselves.
float SOG_maxDistBetweenGaussians
A parameter for initializing 2D/3D SOGs.
Definition: CBeaconMap.h:189
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
Definition: CBeaconMap.cpp:974
CBeaconMap()
Constructor.
Definition: CBeaconMap.cpp:87
A list of TMatchingPair.
Definition: TMatchingPair.h:83
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
Definition: CBeaconMap.h:46
float SOG_separationConstant
Constant used to compute the std.
Definition: CBeaconMap.h:194
void saveToTextFile(const std::string &fil) const
Save a text file with a row per beacon, containing this 11 elements:
void push_back(const CBeacon &m)
Inserts a copy of the given mode into the SOG.
Definition: CBeaconMap.h:105
GLsizei const GLchar ** string
Definition: glext.h:4101
A class used to store a 3D point.
Definition: CPoint3D.h:33
CBeacon & operator[](size_t i)
Access to individual beacons.
Definition: CBeaconMap.h:88
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
const CBeacon & operator[](size_t i) const
Access to individual beacons.
Definition: CBeaconMap.h:76
virtual void determineMatching2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, mrpt::tfest::TMatchingPairList &correspondences, const TMatchingParams &params, TMatchingExtraResults &extraResults) const override
Computes the matching between this and another 2D point map, which includes finding: ...
Definition: CBeaconMap.cpp:792
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
Initilization of default parameters.
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
double rangeStd
The standard deviation used for Beacon ranges likelihood (default=0.08m).
Definition: CBeaconMap.h:125
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
Definition: CBeaconMap.cpp:980
bool insertAsMonteCarlo
Insert a new beacon as a set of montecarlo samples (default=true), or, if false, as a sum of gaussian...
Definition: CBeaconMap.h:145
const CBeacon * getBeaconByID(CBeacon::TBeaconID id) const
Returns a pointer to the beacon with the given ID, or nullptr if it does not exist.
GLsizei GLsizei GLchar * source
Definition: glext.h:4082
virtual bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
Definition: CBeaconMap.cpp:354
void resize(const size_t N)
Resize the number of SOG modes.
Definition: CBeaconMap.cpp:102
mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOptions
std::deque< CBeacon >::const_iterator const_iterator
Definition: CBeaconMap.h:53
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
Definition: CBeaconMap.cpp:145
const_iterator begin() const
Definition: CBeaconMap.h:101
bool saveToMATLABScript3D(const std::string &file, const char *style="b", float confInterval=0.95f) const
Save to a MATLAB script which displays 3D error ellipses for the map.
Definition: CBeaconMap.cpp:922
unsigned int MC_numSamplesPerMeter
Number of particles per meter of range, i.e.
Definition: CBeaconMap.h:157
std::deque< CBeacon > TSequenceBeacons
Definition: CBeaconMap.h:51
double MC_thresholdNegligible
Threshold for the maximum difference from the maximun (log) weight in the set of samples for erasing ...
Definition: CBeaconMap.h:167
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
mrpt::maps::CBeaconMap::TInsertionOptions insertionOptions
float MC_maxStdToGauss
The threshold for the maximum std (X,Y,and Z) before colapsing the particles into a Gaussian PDF (def...
Definition: CBeaconMap.h:162
Parameters for the determination of matchings between point clouds, etc.
#define MAP_DEFINITION_END(_CLASS_NAME_)
void simulateBeaconReadings(const mrpt::poses::CPose3D &in_robotPose, const mrpt::poses::CPoint3D &in_sensorLocationOnRobot, mrpt::obs::CObservationBeaconRanges &out_Observations) const
Simulates a reading toward each of the beacons in the landmarks map, if any.
GLenum const GLfloat * params
Definition: glext.h:3534
const Scalar * const_iterator
Definition: eigen_plugins.h:27
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions...
Definition: CBeacon.h:38
float SOG_thresholdNegligible
Threshold for the maximum difference from the maximun (log) weight in the SOG for erasing a given mod...
Definition: CBeaconMap.h:185
std::deque< CBeacon >::iterator iterator
Definition: CBeaconMap.h:52
size_t size() const
Returns the stored landmarks count.
Definition: CBeaconMap.cpp:98
void computeMatchingWith3DLandmarks(const mrpt::maps::CBeaconMap *otherMap, mrpt::tfest::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: Firsly, the landmarks&#39; descriptor is used to find correspondences, then inconsistent ones removed by looking at their 3D poses.
Definition: CBeaconMap.cpp:845



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