Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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>
18 #include <mrpt/obs/obs_frwds.h>
19 
20 #include <mrpt/maps/link_pragmas.h>
21 
22 namespace mrpt
23 {
24 namespace maps
25 {
27 
28  /** A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
29  * <br>
30  * The individual beacons are defined as mrpt::maps::CBeacon objects.
31  * <br>
32  * When invoking CBeaconMap::insertObservation(), landmarks will be extracted and fused into the map.
33  * The only currently supported observation type is mrpt::obs::CObservationBeaconRanges.
34  * See insertionOptions and likelihoodOptions for parameters used when creating and fusing beacon landmarks.
35  * <br>
36  * Use "TInsertionOptions::insertAsMonteCarlo" to select between 2 different behaviors:
37  * - Initial PDF of beacons: MonteCarlo, after convergence, pass to Gaussians; or
38  * - Initial PDF of beacons: SOG, after convergence, a single Gaussian.
39  *
40  * Refer to the papers: []
41  *
42  * \ingroup mrpt_maps_grp
43  * \sa CMetricMap
44  */
45  class MAPS_IMPEXP CBeaconMap : public mrpt::maps::CMetricMap
46  {
47  // This must be added to any CSerializable derived class:
49 
50  public:
51  typedef std::deque<CBeacon> TSequenceBeacons;
52  typedef std::deque<CBeacon>::iterator iterator;
54 
55  protected:
56  TSequenceBeacons m_beacons; //!< The individual beacons
57 
58  // See docs in base class
59  virtual void internal_clear();
60  virtual bool internal_insertObservation( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL );
61  double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom );
62 
63  public:
64  /** Constructor */
65  CBeaconMap();
66 
67  void resize(const size_t N); //!< Resize the number of SOG modes
68 
69  /** Access to individual beacons */
70  const CBeacon& operator [](size_t i) const {
71  ASSERT_(i<m_beacons.size())
72  return m_beacons[i];
73  }
74  /** Access to individual beacons */
75  const CBeacon& get(size_t i) const{
76  ASSERT_(i<m_beacons.size())
77  return m_beacons[i];
78  }
79  /** Access to individual beacons */
80  CBeacon& operator [](size_t i) {
81  ASSERT_(i<m_beacons.size())
82  return m_beacons[i];
83  }
84  /** Access to individual beacons */
85  CBeacon& get(size_t i) {
86  ASSERT_(i<m_beacons.size())
87  return m_beacons[i];
88  }
89 
90  iterator begin() { return m_beacons.begin(); }
91  const_iterator begin() const { return m_beacons.begin(); }
92  iterator end() { return m_beacons.end(); }
93  const_iterator end() const { return m_beacons.end(); }
94 
95  /** Inserts a copy of the given mode into the SOG */
96  void push_back(const CBeacon& m) {
97  m_beacons.push_back( m );
98  }
99 
100  // See docs in base class
101  float compute3DMatchingRatio(
102  const mrpt::maps::CMetricMap *otherMap,
103  const mrpt::poses::CPose3D &otherMapPose,
104  float maxDistForCorr = 0.10f,
105  float maxMahaDistForCorr = 2.0f
106  ) const;
107 
108  /** With this struct options are provided to the likelihood computations.
109  */
111  {
112  public:
113  /** Initilization of default parameters
114  */
116 
117  /** See utils::CLoadableOptions
118  */
119  void loadFromConfigFile(
120  const mrpt::utils::CConfigFileBase &source,
121  const std::string &section);
122 
123  /** See utils::CLoadableOptions
124  */
125  void dumpToTextStream(mrpt::utils::CStream &out) const;
126 
127  /** The standard deviation used for Beacon ranges likelihood (default=0.08m).
128  */
129  float rangeStd;
130 
131  } likelihoodOptions;
132 
133  /** This struct contains data for choosing the method by which new beacons are inserted in the map.
134  */
136  {
137  public:
138  /** Initilization of default parameters
139  */
141 
142  /** See utils::CLoadableOptions
143  */
144  void loadFromConfigFile(
145  const mrpt::utils::CConfigFileBase &source,
146  const std::string &section);
147 
148  /** See utils::CLoadableOptions
149  */
150  void dumpToTextStream(mrpt::utils::CStream &out) const;
151 
152  /** Insert a new beacon as a set of montecarlo samples (default=true), or, if false, as a sum of gaussians (see mrpt::maps::CBeacon).
153  * \sa MC_performResampling
154  */
156 
157  /** 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.
158  */
159  float maxElevation_deg,minElevation_deg;
160 
161  /** Number of particles per meter of range, i.e. per meter of the "radius of the ring".
162  */
163  unsigned int MC_numSamplesPerMeter;
164 
165  /** The threshold for the maximum std (X,Y,and Z) before colapsing the particles into a Gaussian PDF (default=0,4).
166  */
168 
169  /** Threshold for the maximum difference from the maximun (log) weight in the set of samples for erasing a given sample (default=5).
170  */
172 
173  /** 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.
174  */
176 
177  /** The std.dev. of the Gaussian noise to be added to each sample after resampling, only if MC_performResampling=true.
178  */
180 
181  /** Threshold for the maximum difference from the maximun (log) weight in the SOG for erasing a given mode (default=20).
182  */
184 
185  /** A parameter for initializing 2D/3D SOGs
186  */
188 
189  /** Constant used to compute the std. dev. int the tangent direction when creating the Gaussians.
190  */
192 
193  } insertionOptions;
194 
195  /** Save to a MATLAB script which displays 3D error ellipses for the map.
196  * \param file The name of the file to save the script to.
197  * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
198  * \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)
199  *
200  * \return Returns false if any error occured, true elsewere.
201  */
202  bool saveToMATLABScript3D(
203  const std::string &file,
204  const char *style="b",
205  float confInterval = 0.95f ) const;
206 
207 
208  /** Returns the stored landmarks count.
209  */
210  size_t size() const;
211 
212  // See docs in base class
213  virtual void determineMatching2D(
214  const mrpt::maps::CMetricMap * otherMap,
215  const mrpt::poses::CPose2D & otherMapPose,
216  mrpt::utils::TMatchingPairList & correspondences,
217  const TMatchingParams & params,
218  TMatchingExtraResults & extraResults ) const ;
219 
220  /** Perform a search for correspondences between "this" and another lansmarks map:
221  * Firsly, the landmarks' descriptor is used to find correspondences, then inconsistent ones removed by
222  * looking at their 3D poses.
223  * \param otherMap [IN] The other map.
224  * \param correspondences [OUT] The matched pairs between maps.
225  * \param correspondencesRatio [OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap
226  * \param otherCorrespondences [OUT] Will be returned with a vector containing "true" for the indexes of the other map's landmarks with a correspondence.
227  */
228  void computeMatchingWith3DLandmarks(
229  const mrpt::maps::CBeaconMap *otherMap,
230  mrpt::utils::TMatchingPairList &correspondences,
231  float &correspondencesRatio,
232  std::vector<bool> &otherCorrespondences) const;
233 
234  /** Changes the reference system of the map to a given 3D pose.
235  */
236  void changeCoordinatesReference( const mrpt::poses::CPose3D &newOrg );
237 
238  /** Changes the reference system of the map "otherMap" and save the result in "this" map.
239  */
240  void changeCoordinatesReference( const mrpt::poses::CPose3D &newOrg, const mrpt::maps::CBeaconMap *otherMap );
241 
242 
243  /** Returns true if the map is empty/no observation has been inserted.
244  */
245  bool isEmpty() const;
246 
247  /** Simulates a reading toward each of the beacons in the landmarks map, if any.
248  * \param in_robotPose This robot pose is used to simulate the ranges to each beacon.
249  * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot
250  * \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.
251  * 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.
252  */
253  void simulateBeaconReadings(
254  const mrpt::poses::CPose3D &in_robotPose,
255  const mrpt::poses::CPoint3D &in_sensorLocationOnRobot,
256  mrpt::obs::CObservationBeaconRanges &out_Observations ) const;
257 
258  /** 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).
259  * In the case of this class, these files are generated:
260  * - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks as 3D ellipses.
261  * - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane grid" and the set of ellipsoids in 3D.
262  * - "filNamePrefix"+"_covs.m": A textual representation (see saveToTextFile)
263  */
264  void saveMetricMapRepresentationToFile(
265  const std::string &filNamePrefix ) const;
266 
267  /** Save a text file with a row per beacon, containing this 11 elements:
268  * - X Y Z: Mean values
269  * - VX VY VZ: Variances of each dimension (C11, C22, C33)
270  * - DET2D DET3D: Determinant of the 2D and 3D covariance matrixes.
271  * - C12, C13, C23: Cross covariances
272  */
273  void saveToTextFile(const std::string &fil) const;
274 
275  void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; //!< Returns a 3D object representing the map.
276 
277  const CBeacon * getBeaconByID( CBeacon::TBeaconID id ) const; //!< Returns a pointer to the beacon with the given ID, or NULL if it does not exist.
278  CBeacon * getBeaconByID( CBeacon::TBeaconID id ); //!< Returns a pointer to the beacon with the given ID, or NULL if it does not exist.
279 
280 
282  mrpt::maps::CBeaconMap::TInsertionOptions insertionOpts; //!< Observations insertion options
283  mrpt::maps::CBeaconMap::TLikelihoodOptions likelihoodOpts; //!< Probabilistic observation likelihood options
285 
286  }; // End of class def.
287  DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE( CBeaconMap, CMetricMap ,MAPS_IMPEXP )
288 
289 
290  } // End of namespace
291 } // End of namespace
292 
293 #endif
float MC_thresholdNegligible
Threshold for the maximum difference from the maximun (log) weight in the set of samples for erasing ...
int64_t TBeaconID
The type for the IDs of landmarks.
Definition: maps/CBeacon.h:48
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
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.
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
STL namespace.
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: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, 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.
A class used to store a 3D point.
Definition: CPoint3D.h:32
std::deque< CBeacon > TSequenceBeacons
const_iterator begin() const
const_iterator end() const
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)
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.11 for MRPT 1.3.2 SVN: at Wed May 25 02:34:21 UTC 2016