Main MRPT website > C++ reference for MRPT 1.4.0
maps/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-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 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 #include <mrpt/maps/link_pragmas.h>
20 
21 namespace mrpt
22 {
23 namespace maps
24 {
26 
27  /** A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
28  * <br>
29  * The individual beacons are defined as mrpt::maps::CBeacon objects.
30  * <br>
31  * When invoking CBeaconMap::insertObservation(), landmarks will be extracted and fused into the map.
32  * The only currently supported observation type is mrpt::obs::CObservationBeaconRanges.
33  * See insertionOptions and likelihoodOptions for parameters used when creating and fusing beacon landmarks.
34  * <br>
35  * Use "TInsertionOptions::insertAsMonteCarlo" to select between 2 different behaviors:
36  * - Initial PDF of beacons: MonteCarlo, after convergence, pass to Gaussians; or
37  * - Initial PDF of beacons: SOG, after convergence, a single Gaussian.
38  *
39  * Refer to the papers: []
40  *
41  * \ingroup mrpt_maps_grp
42  * \sa CMetricMap
43  */
44  class MAPS_IMPEXP CBeaconMap : public mrpt::maps::CMetricMap
45  {
46  // This must be added to any CSerializable derived class:
48 
49  public:
50  typedef std::deque<CBeacon> TSequenceBeacons;
53 
54  protected:
55  TSequenceBeacons m_beacons; //!< The individual beacons
56 
57  // See docs in base class
58  virtual void internal_clear() MRPT_OVERRIDE;
59  virtual bool internal_insertObservation( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
60  double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) MRPT_OVERRIDE;
61 
62  public:
63  /** Constructor */
64  CBeaconMap();
65 
66  void resize(const size_t N); //!< Resize the number of SOG modes
67 
68  /** Access to individual beacons */
69  const CBeacon& operator [](size_t i) const {
70  ASSERT_(i<m_beacons.size())
71  return m_beacons[i];
72  }
73  /** Access to individual beacons */
74  const CBeacon& get(size_t i) const{
75  ASSERT_(i<m_beacons.size())
76  return m_beacons[i];
77  }
78  /** Access to individual beacons */
79  CBeacon& operator [](size_t i) {
80  ASSERT_(i<m_beacons.size())
81  return m_beacons[i];
82  }
83  /** Access to individual beacons */
84  CBeacon& get(size_t i) {
85  ASSERT_(i<m_beacons.size())
86  return m_beacons[i];
87  }
88 
89  iterator begin() { return m_beacons.begin(); }
90  const_iterator begin() const { return m_beacons.begin(); }
91  iterator end() { return m_beacons.end(); }
92  const_iterator end() const { return m_beacons.end(); }
93 
94  /** Inserts a copy of the given mode into the SOG */
95  void push_back(const CBeacon& m) {
96  m_beacons.push_back( m );
97  }
98 
99  // See docs in base class
100  float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
101 
102  /** With this struct options are provided to the likelihood computations */
104  {
105  public:
106  /** Initilization of default parameters
107  */
109 
110  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
111  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
112 
113  /** The standard deviation used for Beacon ranges likelihood (default=0.08m).
114  */
115  float rangeStd;
116  } likelihoodOptions;
117 
118  /** This struct contains data for choosing the method by which new beacons are inserted in the map.
119  */
121  {
122  public:
123  /** Initilization of default parameters */
125  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
126  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
127 
128  /** Insert a new beacon as a set of montecarlo samples (default=true), or, if false, as a sum of gaussians (see mrpt::maps::CBeacon).
129  * \sa MC_performResampling
130  */
132 
133  /** Minimum and maximum elevation angles (in degrees) for inserting new beacons at the first observation: the default values (both 0), makes the beacons to be in the same horizontal plane that the sensors, that is, 2D SLAM - the min/max values are -90/90.
134  */
135  float maxElevation_deg,minElevation_deg;
136 
137  /** Number of particles per meter of range, i.e. per meter of the "radius of the ring".
138  */
139  unsigned int MC_numSamplesPerMeter;
140 
141  /** The threshold for the maximum std (X,Y,and Z) before colapsing the particles into a Gaussian PDF (default=0,4).
142  */
144 
145  /** Threshold for the maximum difference from the maximun (log) weight in the set of samples for erasing a given sample (default=5).
146  */
148 
149  /** If set to false (default), the samples will be generated the first time a beacon is observed, and their weights just updated subsequently - if set to "true", fewer samples will be required since the particles will be resamples when necessary, and a small "noise" will be added to avoid depletion.
150  */
152 
153  /** The std.dev. of the Gaussian noise to be added to each sample after resampling, only if MC_performResampling=true.
154  */
156 
157  /** Threshold for the maximum difference from the maximun (log) weight in the SOG for erasing a given mode (default=20).
158  */
160 
161  /** A parameter for initializing 2D/3D SOGs
162  */
164 
165  /** Constant used to compute the std. dev. int the tangent direction when creating the Gaussians.
166  */
168 
169  } insertionOptions;
170 
171  /** Save to a MATLAB script which displays 3D error ellipses for the map.
172  * \param file The name of the file to save the script to.
173  * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
174  * \param stdCount The ellipsoids will be drawn from the center to a given confidence interval in [0,1], e.g. 2 sigmas=0.95 (default is 2std = 0.95 confidence intervals)
175  *
176  * \return Returns false if any error occured, true elsewere.
177  */
178  bool saveToMATLABScript3D(
179  const std::string &file,
180  const char *style="b",
181  float confInterval = 0.95f ) const;
182 
183 
184  /** Returns the stored landmarks count.
185  */
186  size_t size() const;
187 
188  // See docs in base class
189  virtual void determineMatching2D(
190  const mrpt::maps::CMetricMap * otherMap,
191  const mrpt::poses::CPose2D & otherMapPose,
192  mrpt::utils::TMatchingPairList & correspondences,
193  const TMatchingParams & params,
194  TMatchingExtraResults & extraResults ) const MRPT_OVERRIDE;
195 
196  /** Perform a search for correspondences between "this" and another lansmarks map:
197  * Firsly, the landmarks' descriptor is used to find correspondences, then inconsistent ones removed by
198  * looking at their 3D poses.
199  * \param otherMap [IN] The other map.
200  * \param correspondences [OUT] The matched pairs between maps.
201  * \param correspondencesRatio [OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap
202  * \param otherCorrespondences [OUT] Will be returned with a vector containing "true" for the indexes of the other map's landmarks with a correspondence.
203  */
204  void computeMatchingWith3DLandmarks(
205  const mrpt::maps::CBeaconMap *otherMap,
206  mrpt::utils::TMatchingPairList &correspondences,
207  float &correspondencesRatio,
208  std::vector<bool> &otherCorrespondences) const;
209 
210  /** Changes the reference system of the map to a given 3D pose.
211  */
212  void changeCoordinatesReference( const mrpt::poses::CPose3D &newOrg );
213 
214  /** Changes the reference system of the map "otherMap" and save the result in "this" map.
215  */
216  void changeCoordinatesReference( const mrpt::poses::CPose3D &newOrg, const mrpt::maps::CBeaconMap *otherMap );
217 
218 
219  /** Returns true if the map is empty/no observation has been inserted.
220  */
221  bool isEmpty() const MRPT_OVERRIDE;
222 
223  /** Simulates a reading toward each of the beacons in the landmarks map, if any.
224  * \param in_robotPose This robot pose is used to simulate the ranges to each beacon.
225  * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot
226  * \param out_Observations The results will be stored here. NOTICE that the fields "CObservationBeaconRanges::minSensorDistance","CObservationBeaconRanges::maxSensorDistance" and "CObservationBeaconRanges::stdError" MUST BE FILLED OUT before calling this function.
227  * An observation will be generated for each beacon in the map, but notice that some of them may be missed if out of the sensor maximum range.
228  */
229  void simulateBeaconReadings(
230  const mrpt::poses::CPose3D &in_robotPose,
231  const mrpt::poses::CPoint3D &in_sensorLocationOnRobot,
232  mrpt::obs::CObservationBeaconRanges &out_Observations ) const;
233 
234  /** 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).
235  * In the case of this class, these files are generated:
236  * - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks as 3D ellipses.
237  * - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane grid" and the set of ellipsoids in 3D.
238  * - "filNamePrefix"+"_covs.m": A textual representation (see saveToTextFile)
239  */
240  void saveMetricMapRepresentationToFile(const std::string &filNamePrefix ) const MRPT_OVERRIDE;
241 
242  /** Save a text file with a row per beacon, containing this 11 elements:
243  * - X Y Z: Mean values
244  * - VX VY VZ: Variances of each dimension (C11, C22, C33)
245  * - DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
246  * - C12, C13, C23: Cross covariances
247  */
248  void saveToTextFile(const std::string &fil) const;
249 
250  void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE; //!< Returns a 3D object representing the map.
251 
252  const CBeacon * getBeaconByID( CBeacon::TBeaconID id ) const; //!< Returns a pointer to the beacon with the given ID, or NULL if it does not exist.
253  CBeacon * getBeaconByID( CBeacon::TBeaconID id ); //!< Returns a pointer to the beacon with the given ID, or NULL if it does not exist.
254 
255 
257  mrpt::maps::CBeaconMap::TInsertionOptions insertionOpts; //!< Observations insertion options
258  mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOpts; //!< Probabilistic observation likelihood options
260 
261  }; // End of class def.
263 
264 
265  } // End of namespace
266 } // End of namespace
267 
268 #endif
float MC_thresholdNegligible
Threshold for the maximum difference from the maximun (log) weight in the set of samples for erasing ...
Parameters for CMetricMap::compute3DMatchingRatio()
const_iterator end() const
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
This struct contains data for choosing the method by which new beacons are inserted in the map.
With this struct options are provided to the likelihood computations.
Scalar * iterator
Definition: eigen_plugins.h:23
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
std::deque< CBeacon >::iterator iterator
const Scalar * const_iterator
Definition: eigen_plugins.h:24
bool MC_performResampling
If set to false (default), the samples will be generated the first time a beacon is observed,...
CBeacon & get(size_t i)
Access to individual beacons.
This class allows loading and storing values and vectors of different types from a configuration text...
TSequenceBeacons m_beacons
The individual beacons.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
const CBeacon & get(size_t i) const
Access to individual beacons.
#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...
Additional results from the determination of matchings between point clouds, etc.,...
float SOG_maxDistBetweenGaussians
A parameter for initializing 2D/3D SOGs.
A list of TMatchingPair.
Definition: TMatchingPair.h:66
float rangeStd
The standard deviation used for Beacon ranges likelihood (default=0.08m).
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian,...
float SOG_separationConstant
Constant used to compute the std.
void push_back(const CBeacon &m)
Inserts a copy of the given mode into the SOG.
std::deque< CBeacon > TSequenceBeacons
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...
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:38
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose.
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
bool insertAsMonteCarlo
Insert a new beacon as a set of montecarlo samples (default=true), or, if false, as a sum of gaussian...
#define ASSERT_(f)
const_iterator begin() const
std::deque< CBeacon >::const_iterator const_iterator
unsigned int MC_numSamplesPerMeter
Number of particles per meter of range, i.e.
float MC_maxStdToGauss
The threshold for the maximum std (X,Y,and Z) before colapsing the particles into a Gaussian PDF (def...
Parameters for the determination of matchings between point clouds, etc.
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions.
Definition: maps/CBeacon.h:40
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ...
float SOG_thresholdNegligible
Threshold for the maximum difference from the maximun (log) weight in the SOG for erasing a given mod...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...



Page generated by Doxygen 1.8.15 for MRPT 1.4.0 SVN: at Sat Aug 3 20:09:00 UTC 2019