Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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;
51  typedef std::deque<CBeacon>::iterator iterator;
52  typedef std::deque<CBeacon>::const_iterator const_iterator;
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.
262  DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE( CBeaconMap, CMetricMap ,MAPS_IMPEXP )
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()
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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...
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.
std::deque< CBeacon >::iterator iterator
bool MC_performResampling
If set to false (default), the samples will be generated the first time a beacon is observed...
This class allows loading and storing values and vectors of different types from a configuration text...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
#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., apart from the pairings themselves.
float SOG_maxDistBetweenGaussians
A parameter for initializing 2D/3D SOGs.
A list of TMatchingPair.
Definition: TMatchingPair.h:78
float rangeStd
The standard deviation used for Beacon ranges likelihood (default=0.08m).
VALUE & operator[](const KEY &key)
Write/read via [i] operator, that creates an element if it didn't exist already.
Definition: ts_hash_map.h:123
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
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
const_iterator begin() const
GLsizeiptr size
Definition: glew.h:1586
const_iterator end() const
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
Declares a virtual base class for all metric maps storage classes.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
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)
GLfloat * params
Definition: glew.h:1436
const GLdouble * m
Definition: glew.h:5094
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() ...
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
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...
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018