Main MRPT website > C++ reference
MRPT logo
CLandmarksMap.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-2014, 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 CLandmarksMap_H
10 #define CLandmarksMap_H
11 
13 #include <mrpt/slam/CMetricMap.h>
14 #include <mrpt/slam/CLandmark.h>
20 #include <mrpt/math/CMatrix.h>
23 
24 namespace mrpt
25 {
26 namespace slam
27 {
28  class CObservationBeaconRanges;
30 
31  /** Internal use.
32  */
33  typedef std::vector<CLandmark> TSequenceLandmarks;
34 
36 
37  /** A class for storing a map of 3D probabilistic landmarks.
38  * <br>
39  * Currently these types of landmarks are defined: (see mrpt::slam::CLandmark)
40  * - For "visual landmarks" from images: features with associated descriptors.
41  * - For laser scanners: each of the range measuremnts, as "occupancy" landmarks.
42  * - For grid maps: "Panoramic descriptor" feature points.
43  * - For range-only localization and SLAM: Beacons. It is also supported the simulation of expected beacon-to-sensor readings, observation likelihood,...
44  * <br>
45  * <b>How to load landmarks from observations:</b><br>
46  * When invoking CLandmarksMap::insertObservation(), the values in CLandmarksMap::insertionOptions will
47  * determinate the kind of landmarks that will be extracted and fused into the map. Supported feature
48  * extraction processes are listed next:
49  *
50  <table>
51  <tr> <td><b>Observation class:</b></td> <td><b>Generated Landmarks:</b></td> <td><b>Comments:</b></td> </tr>
52  <tr> <td>CObservationImage</td> <td>vlSIFT</td> <td>1) A SIFT feature is created for each SIFT detected in the image,
53  <br>2) Correspondences between SIFTs features and existing ones are finded by computeMatchingWith3DLandmarks,
54  <br>3) The corresponding feaures are fused, and the new ones added, with an initial uncertainty according to insertionOptions</td> </tr>
55  <tr> <td>CObservationStereoImages</td> <td>vlSIFT</td> <td> Each image is separately procesed by the method for CObservationImage observations </td> </tr>
56  <tr> <td>CObservationStereoImages</td> <td>vlColor</td> <td> TODO... </td> </tr>
57  <tr> <td>CObservation2DRangeScan</td> <td>glOccupancy</td> <td> A landmark is added for each range in the scan, with its appropiate covariance matrix derived from the jacobians matrixes. </td> </tr>
58  </table>
59  *
60  * \sa CMetricMap
61  * \ingroup mrpt_vision_grp
62  */
64  {
65  // This must be added to any CSerializable derived class:
67 
68  private:
69 
70  virtual void internal_clear();
71 
72  /** Insert the observation information into this map. This method must be implemented
73  * in derived classes.
74  * \param obs The observation
75  * \param robotPose The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg)
76  *
77  * \sa CObservation::insertObservationInto
78  */
79  virtual bool internal_insertObservation( const CObservation *obs, const CPose3D *robotPose = NULL );
80 
81  public:
82 
83  static mrpt::utils::TColorf COLOR_LANDMARKS_IN_3DSCENES; //!< The color of landmark ellipsoids in CLandmarksMap::getAs3DObject
84 
86 
87 
88  /** The list of landmarks: the wrapper class is just for maintaining the KD-Tree representation
89  */
91  {
92  private:
93  /** The actual list:
94  */
96 
97  /** A grid-map with the set of landmarks falling into each cell.
98  * \todo Use the KD-tree instead?
99  */
101 
102  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
103  * \sa getLargestDistanceFromOrigin
104  */
106 
107  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
108  * \sa getLargestDistanceFromOrigin
109  */
111 
112  public:
113  /** Default constructor
114  */
116 
118  inline iterator begin() { return m_landmarks.begin(); };
119  inline iterator end() { return m_landmarks.end(); };
120  void clear();
121  inline size_t size() const { return m_landmarks.size(); };
122 
124  inline const_iterator begin() const { return m_landmarks.begin(); };
125  inline const_iterator end() const { return m_landmarks.end(); };
126 
127  /** The object is copied, thus the original copy passed as a parameter can be released.
128  */
129  void push_back( const CLandmark &lm);
130  CLandmark* get(unsigned int indx);
131  const CLandmark* get(unsigned int indx) const;
132  void isToBeModified(unsigned int indx);
133  void hasBeenModified(unsigned int indx);
134  void hasBeenModifiedAll();
135  void erase(unsigned int indx);
136 
137  CDynamicGrid<vector_int>* getGrid() { return &m_grid; }
138 
139  /** Returns the landmark with a given landmrk ID, or NULL if not found
140  */
141  const CLandmark* getByID( CLandmark::TLandmarkID ID ) const;
142 
143  /** Returns the landmark with a given beacon ID, or NULL if not found
144  */
145  const CLandmark* getByBeaconID( unsigned int ID ) const;
146 
147  /** This method returns the largest distance from the origin to any of the points, such as a sphere centered at the origin with this radius cover ALL the points in the map (the results are buffered, such as, if the map is not modified, the second call will be much faster than the first one).
148  */
149  float getLargestDistanceFromOrigin() const;
150 
151  } landmarks;
152 
153  /** Constructor
154  */
155  CLandmarksMap();
156 
157  /** Virtual destructor.
158  */
159  virtual ~CLandmarksMap();
160 
161 
162  /**** FAMD ***/
163  /** Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks
164  */
165  static std::map<std::pair<mrpt::slam::CLandmark::TLandmarkID, mrpt::slam::CLandmark::TLandmarkID>, double> _mEDD;
167  static bool _maxIDUpdated;
168 
170  /**** END FAMD *****/
171 
172  // See docs in base class
173  float compute3DMatchingRatio(
174  const CMetricMap *otherMap,
175  const CPose3D &otherMapPose,
176  float maxDistForCorr = 0.10f,
177  float maxMahaDistForCorr = 2.0f
178  ) const;
179 
180  /** With this struct options are provided to the observation insertion process.
181  */
183  {
184  public:
185  /** Initilization of default parameters
186  */
188 
189  /** See utils::CLoadableOptions
190  */
191  void loadFromConfigFile(
192  const mrpt::utils::CConfigFileBase &source,
193  const std::string &section);
194 
195  /** See utils::CLoadableOptions
196  */
197  void dumpToTextStream(CStream &out) const;
198 
199  /** If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D features.
200  */
202 
203  /** If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D features.
204  */
206 
207  /** If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for each range.
208  */
210 
211  /** [For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as correspondence (Default=0.4)
212  */
214 
215  /** [For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0.5)
216  */
218 
219  /****************************************** FAMD ******************************************/
220  /** [For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as correspondence (Default=200)
221  */
223 
224  /** [For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method))
225  * 0: Our method -> Euclidean Distance between Descriptors and 3D position
226  * 1: Sim, Elinas, Griffin, Little -> Euclidean Distance between Descriptors
227  */
228  unsigned int SIFTMatching3DMethod;
229 
230  /** [For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method))
231  * 0: Our method -> Euclidean Distance between Descriptors and 3D position
232  * 1: Sim, Elinas, Griffin, Little -> 3D position
233  */
234  unsigned int SIFTLikelihoodMethod;
235 
236  /****************************************** END FAMD ******************************************/
237 
238  /** [For SIFT landmarks only] The distance (in meters) of the mean value of landmarks, for the initial position PDF (Default = 3m)
239  */
241 
242  /** [For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perpendicular to the main directiom (Default = 0.05f)
243  */
245 
246  /** [For SIFT landmarks only] The standard deviation (in pixels) for the SIFTs detector (This is used for the Jacobbian to project stereo images to 3D)
247  */
248  float SIFTs_stdXY, SIFTs_stdDisparity;
249 
250  /** Number of points to extract in the image
251  */
253 
254  /** Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation.
255  */
257 
258  /** Maximum distance (in pixels) from a point to a certain epipolar line to be considered a potential match.
259  */
261 
262  /** Indicates if the images (as well as the SIFT detected features) should be shown in a window.
263  */
265 
266  /** Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map.
267  * \note There exists another \a SIFT_feat_options field in the \a likelihoodOptions member.
268  * \note All parameters of this field can be loaded from a config file. See mrpt::vision::CFeatureExtraction::TOptions for the names of the expected fields.
269  */
271 
272  } insertionOptions;
273 
274  /** With this struct options are provided to the likelihood computations.
275  */
277  {
278  public:
279  /** Initilization of default parameters
280  */
282 
283  /** See utils::CLoadableOptions
284  */
285  void loadFromConfigFile(
286  const mrpt::utils::CConfigFileBase &source,
287  const std::string &section);
288 
289  /** See utils::CLoadableOptions
290  */
291  void dumpToTextStream(CStream &out) const;
292 
293  /** The number of rays from a 2D range scan will be decimated by this factor (default = 1, no decimation)
294  */
296 
298 
300 
302 
304 
305  /** Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood computation.
306  */
308 
309  /** The standard deviation used for Beacon ranges likelihood (default=0.08).
310  */
312 
313  /** The ratio between gaussian and uniform distribution (default=1).
314  */
315  float alphaRatio;
316 
317  /** Maximun reliable beacon range value (default=20)
318  */
320 
321  /** This struct store de GPS longitude, latitude (in degrees ) and altitude (in meters) for the first GPS observation
322  * compose with de sensor position on the robot.
323  */
325  {
326  public:
327  TGPSOrigin();
328 
329  /** Longitud del Origen del GPS (en grados)
330  */
331  double longitude;
332 
333  /** Latitud del Origen del GPS (en grados)
334  */
335  double latitude;
336 
337  /** Altitud del Origen del GPS (en metros)
338  */
339  double altitude;
340 
341  /** Estas tres opciones sirven para encajar mapas de GPS con posiciones absolutas con
342  * mapas de otros sensores (como laser :D) se obtienen facilmente con el programa matlab map_matching
343  * ang : Rotación del mapa del GPS (para encajarlo en grados)
344  * x_shift: Desplazamiento en x relativo al robot (en metros)
345  * y_shift: Desplazamiento en y relativo al robot (en metros)
346  */
347  double ang,
348  x_shift,
349  y_shift;
350 
351  /** Número mínimo de satelites para tener en cuenta los datos
352  */
353  unsigned int min_sat;
354 
355  } GPSOrigin;
356 
357  /** A constant "sigma" for GPS localization data (in meters)
358  */
359  float GPS_sigma;
360 
361  /** Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map.
362  * \note There exists another \a SIFT_feat_options field in the \a insertionOptions member.
363  * \note All parameters of this field can be loaded from a config file. See mrpt::vision::CFeatureExtraction::TOptions for the names of the expected fields.
364  */
366 
367  } likelihoodOptions;
368 
369  /** This struct stores extra results from invoking insertObservation
370  */
372  {
373  /** The number of SIFT detected in left and right images respectively
374  */
375 
376  unsigned int nSiftL, nSiftR;
377 
378 
379  } insertionResults;
380 
381  /** With this struct options are provided to the fusion process.
382  */
384  {
385  /** Initialization
386  */
387  TFuseOptions();
388 
389  /** Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds.
390  */
391  unsigned int minTimesSeen;
392 
393  /** See "minTimesSeen"
394  */
396 
397  } fuseOptions;
398 
399 
400  /** Save to a text file.
401  * In line "i" there are the (x,y,z) mean values of the i'th landmark + type of landmark + # times seen + timestamp + RGB/descriptor + ID
402  *
403  * Returns false if any error occured, true elsewere.
404  */
405  bool saveToTextFile(std::string file);
406 
407  /** Save to a MATLAB script which displays 2D error ellipses for the map (top-view, projection on the XY plane).
408  * \param file The name of the file to save the script to.
409  * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
410  * \param stdCount The ellipsoids will be drawn from the center to "stdCount" times the "standard deviations". (default is 2std = 95% confidence intervals)
411  *
412  * \return Returns false if any error occured, true elsewere.
413  */
414  bool saveToMATLABScript2D(
415  std::string file,
416  const char *style="b",
417  float stdCount = 2.0f );
418 
419  /** Save to a MATLAB script which displays 3D error ellipses for the map.
420  * \param file The name of the file to save the script to.
421  * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
422  * \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)
423  *
424  * \return Returns false if any error occured, true elsewere.
425  */
426  bool saveToMATLABScript3D(
427  std::string file,
428  const char *style="b",
429  float confInterval = 0.95f ) const ;
430 
431  /** Returns the stored landmarks count.
432  */
433  size_t size() const;
434 
435  /** Computes the (logarithmic) likelihood function for a sensed observation "o" according to "this" map.
436  * This is the implementation of the algorithm reported in the paper:
437  <em>J.L. Blanco, J. Gonzalez, and J.A. Fernandez-Madrigal, "A Consensus-based Approach for Estimating the Observation Likelihood of Accurate Range Sensors", in IEEE International Conference on Robotics and Automation (ICRA), Rome (Italy), Apr 10-14, 2007</em>
438  */
439  double computeLikelihood_RSLC_2007( const CLandmarksMap *s, const CPose2D &sensorPose);
440 
441  /** Loads into this landmarks map the SIFT features extracted from an image observation (Previous contents of map will be erased)
442  * The robot is assumed to be at the origin of the map.
443  * Some options may be applicable from "insertionOptions" (insertionOptions.SIFTsLoadDistanceOfTheMean)
444  *
445  * \param feat_options Optionally, you can pass here parameters for changing the default SIFT detector settings.
446  */
447  void loadSiftFeaturesFromImageObservation(
448  const CObservationImage &obs,
450  );
451 
452  /** Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of stereo-image (Previous contents of map will be erased)
453  * The robot is assumed to be at the origin of the map.
454  * Some options may be applicable from "insertionOptions"
455  *
456  * \param feat_options Optionally, you can pass here parameters for changing the default SIFT detector settings.
457  */
458  void loadSiftFeaturesFromStereoImageObservation(
459  const CObservationStereoImages &obs,
462  );
463 
464  /** Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous contents of map will be erased)
465  * \param obs The observation
466  * \param robotPose The robot pose in the map (Default = the origin)
467  * Some options may be applicable from "insertionOptions"
468  */
469  void loadOccupancyFeaturesFrom2DRangeScan(
470  const CObservation2DRangeScan &obs,
471  const CPose3D *robotPose = NULL,
472  unsigned int downSampleFactor = 1);
473 
474 
475  /** Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the world being modeled with this map.
476  *
477  * In the current implementation, this method behaves in a different way according to the nature of
478  * the observation's class:
479  * - "mrpt::slam::CObservation2DRangeScan": This calls "computeLikelihood_RSLC_2007".
480  * - "mrpt::slam::CObservationStereoImages": This calls "computeLikelihood_SIFT_LandmarkMap".
481  *
482  * \param takenFrom The robot's pose the observation is supposed to be taken from.
483  * \param obs The observation.
484  * \return This method returns a likelihood value > 0.
485  *
486  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
487  */
488  double computeObservationLikelihood( const CObservation *obs, const CPose3D &takenFrom );
489 
490  // See docs in base class
491  void computeMatchingWith2D(
492  const CMetricMap *otherMap,
493  const CPose2D &otherMapPose,
494  float maxDistForCorrespondence,
495  float maxAngularDistForCorrespondence,
496  const CPose2D &angularDistPivotPoint,
497  TMatchingPairList &correspondences,
498  float &correspondencesRatio,
499  float *sumSqrDist = NULL,
500  bool onlyKeepTheClosest = false,
501  bool onlyUniqueRobust = false ) const;
502 
503  /** Perform a search for correspondences between "this" and another lansmarks map:
504  * In this class, the matching is established solely based on the landmarks' IDs.
505  * \param otherMap [IN] The other map.
506  * \param correspondences [OUT] The matched pairs between maps.
507  * \param correspondencesRatio [OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap
508  * \param otherCorrespondences [OUT] Will be returned with a vector containing "true" for the indexes of the other map's landmarks with a correspondence.
509  */
510  void computeMatchingWith3DLandmarks(
511  const mrpt::slam::CLandmarksMap *otherMap,
512  TMatchingPairList &correspondences,
513  float &correspondencesRatio,
514  std::vector<bool> &otherCorrespondences) const;
515 
516  /** Changes the reference system of the map to a given 3D pose.
517  */
518  void changeCoordinatesReference( const CPose3D &newOrg );
519 
520  /** Changes the reference system of the map "otherMap" and save the result in "this" map.
521  */
522  void changeCoordinatesReference( const CPose3D &newOrg, const mrpt::slam::CLandmarksMap *otherMap );
523 
524  /** Fuses the contents of another map with this one, updating "this" object with the result.
525  * This process involves fusing corresponding landmarks, then adding the new ones.
526  * \param other The other landmarkmap, whose landmarks will be inserted into "this"
527  * \param justInsertAllOfThem If set to "true", all the landmarks in "other" will be inserted into "this" without checking for possible correspondences (may appear duplicates ones, etc...)
528  */
529  void fuseWith( CLandmarksMap &other, bool justInsertAllOfThem = false );
530 
531  /** Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
532  * See paper: JJAA 2006
533  */
534  double computeLikelihood_SIFT_LandmarkMap( CLandmarksMap *map,
535  TMatchingPairList *correspondences = NULL,
536  std::vector<bool> *otherCorrespondences = NULL);
537 
538  /** Returns true if the map is empty/no observation has been inserted.
539  */
540  bool isEmpty() const;
541 
542  /** Simulates a noisy reading toward each of the beacons in the landmarks map, if any.
543  * \param in_robotPose This robot pose is used to simulate the ranges to each beacon.
544  * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot
545  * \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.
546  * 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.
547  */
548  void simulateBeaconReadings(
549  const CPose3D &in_robotPose,
550  const CPoint3D &in_sensorLocationOnRobot,
551  CObservationBeaconRanges &out_Observations ) const;
552 
553  /** Simulates a noisy bearing-range observation of all the beacons (landamrks with type glBeacon) in the landmarks map, if any.
554  * \param[in] robotPose The robot pose.
555  * \param[in] sensorLocationOnRobot The 3D position of the sensor on the robot
556  * \param[out] observations The results will be stored here.
557  * \param[in] sensorDetectsIDs If this is set to false, all the landmarks will be sensed with INVALID_LANDMARK_ID as ID.
558  * \param[in] stdRange The sigma of the sensor noise in range (meters).
559  * \param[in] stdYaw The sigma of the sensor noise in yaw (radians).
560  * \param[in] stdPitch The sigma of the sensor noise in pitch (radians).
561  * \param[out] real_associations If it's not a NULL pointer, this will contain at the return the real indices of the landmarks in the map in the same order than they appear in out_Observations. Useful when sensorDetectsIDs=false. Spurious readings are assigned a std::string::npos (=-1) index.
562  * \param[in] spurious_count_mean The mean number of spurious measurements (uniformly distributed in range & angle) to generate. The number of spurious is generated by rounding a random Gaussin number. If both this mean and the std are zero (the default) no spurious readings are generated.
563  * \param[in] spurious_count_std Read spurious_count_mean above.
564  *
565  * \note The fields "CObservationBearingRange::fieldOfView_*","CObservationBearingRange::maxSensorDistance" and "CObservationBearingRange::minSensorDistance" MUST BE FILLED OUT before calling this function.
566  * \note At output, the observation will have CObservationBearingRange::validCovariances set to "false" and the 3 sensor_std_* members correctly set to their values.
567  * 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 or field of view-
568  */
569  void simulateRangeBearingReadings(
570  const CPose3D &robotPose,
571  const CPose3D &sensorLocationOnRobot,
572  CObservationBearingRange &observations,
573  bool sensorDetectsIDs = true,
574  const float stdRange = 0.01f,
575  const float stdYaw = DEG2RAD(0.1f),
576  const float stdPitch = DEG2RAD(0.1f),
577  vector_size_t *real_associations = NULL,
578  const double spurious_count_mean = 0,
579  const double spurious_count_std = 0
580  ) const;
581 
582 
583  /** 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).
584  * In the case of this class, these files are generated:
585  * - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks as 3D ellipses.
586  * - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane grid" and the set of ellipsoids in 3D.
587  */
588  void saveMetricMapRepresentationToFile(
589  const std::string &filNamePrefix ) const;
590 
591  /** Returns a 3D object representing the map.
592  * \sa COLOR_LANDMARKS_IN_3DSCENES
593  */
594  void getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
595 
596  /** This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
597  * This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
598  */
599  virtual void auxParticleFilterCleanUp();
600 
601  }; // End of class def.
602 
603 
604  } // End of namespace
605 } // End of namespace
606 
607 #endif
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:63
double longitude
Longitud del Origen del GPS (en grados)
TSequenceLandmarks m_landmarks
The actual list:
Definition: CLandmarksMap.h:95
int SIFTs_numberOfKLTKeypoints
Number of points to extract in the image.
float beaconMaxRange
Maximun reliable beacon range value (default=20)
double latitude
Latitud del Origen del GPS (en grados)
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
unsigned int minTimesSeen
Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds...
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
unsigned int SIFTLikelihoodMethod
[For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 0: Our method -...
Scalar * iterator
Definition: eigen_plugins.h:23
float alphaRatio
The ratio between gaussian and uniform distribution (default=1).
float SIFTs_stdXY
[For SIFT landmarks only] The standard deviation (in pixels) for the SIFTs detector (This is used for...
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
unsigned int SIFTMatching3DMethod
[For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method)) 0: Our method -> E...
EIGEN_STRONG_INLINE void push_back(Scalar val)
Insert an element at the end of the container (for 1D vectors/arrays)
mrpt::slam::CLandmark landmark_type
Definition: CLandmarksMap.h:85
The set of parameters for all the detectors & descriptor algorithms.
int SIFTs_decimation
Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood compu...
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...
const Scalar * const_iterator
Definition: eigen_plugins.h:24
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map...
bool insert_SIFTs_from_stereo_images
If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D ...
With this struct options are provided to the likelihood computations.
With this struct options are provided to the observation insertion process.
bool insert_Landmarks_from_range_scans
If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for ...
This class allows loading and storing values and vectors of different types from a configuration text...
unsigned int min_sat
Número mínimo de satelites para tener en cuenta los datos.
std::vector< CLandmark > TSequenceLandmarks
Internal use.
Definition: CLandmarksMap.h:29
float SIFTsLoadEllipsoidWidth
[For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perp...
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:90
bool PLOT_IMAGES
Indicates if the images (as well as the SIFT detected features) should be shown in a window...
This struct store de GPS longitude, latitude (in degrees ) and altitude (in meters) for the first GPS...
TSequenceLandmarks::const_iterator const_iterator
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:36
float SIFTs_stereo_maxDepth
Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation.
#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...
std::vector< size_t > vector_size_t
Scale Invariant Feature Transform [LOWE&#39;04].
double altitude
Altitud del Origen del GPS (en metros)
A list of TMatchingPair.
Definition: TMatchingPair.h:60
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:61
int64_t TLandmarkID
The type for the IDs of landmarks.
Definition: CLandmark.h:46
The list of landmarks: the wrapper class is just for maintaining the KD-Tree representation.
Definition: CLandmarksMap.h:90
static std::map< std::pair< mrpt::slam::CLandmark::TLandmarkID, mrpt::slam::CLandmark::TLandmarkID >, double > _mEDD
Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks.
float beaconRangesStd
The standard deviation used for Beacon ranges likelihood (default=0.08).
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:40
A class used to store a 3D point.
Definition: CPoint3D.h:33
float SiftLikelihoodThreshold
[For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0...
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
bool insert_SIFTs_from_monocular_images
If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D feature...
float SiftEDDThreshold
[For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as corres...
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
CDynamicGrid< vector_int > m_grid
A grid-map with the set of landmarks falling into each cell.
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...
static mrpt::utils::TColorf COLOR_LANDMARKS_IN_3DSCENES
The color of landmark ellipsoids in CLandmarksMap::getAs3DObject.
Definition: CLandmarksMap.h:83
float GPS_sigma
A constant "sigma" for GPS localization data (in meters)
float SiftCorrRatioThreshold
[For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as c...
This struct stores extra results from invoking insertObservation.
float SIFTs_epipolar_TH
Maximum distance (in pixels) from a point to a certain epipolar line to be considered a potential mat...
float SIFTsLoadDistanceOfTheMean
[For SIFT landmarks only] The distance (in meters) of the mean value of landmarks, for the initial position PDF (Default = 3m)
size_t size(const MATRIXLIKE &m, int dim)
Definition: bits.h:46
A class used to store a 2D pose.
Definition: CPose2D.h:35
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:71
A RGB color - floats in the range [0,1].
Definition: TColor.h:51
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
unsigned int rangeScan2D_decimation
The number of rays from a 2D range scan will be decimated by this factor (default = 1...
With this struct options are provided to the fusion process.
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map...
static mrpt::slam::CLandmark::TLandmarkID _mapMaxID
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
float ellapsedTime
See "minTimesSeen".



Page generated by Doxygen 1.8.14 for MRPT 1.1.0 SVN: at lun oct 28 00:54:49 CET 2019 Hosted on:
SourceForge.net Logo