Main MRPT website > C++ reference for MRPT 1.4.0
maps/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-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 CLandmarksMap_H
10 #define CLandmarksMap_H
11 
13 #include <mrpt/maps/CMetricMap.h>
14 #include <mrpt/maps/CLandmark.h>
20 #include <mrpt/math/CMatrix.h>
23 #include <mrpt/obs/obs_frwds.h>
24 
25 namespace mrpt
26 {
27 namespace maps
28 {
29  namespace internal { typedef std::vector<CLandmark> TSequenceLandmarks; }
30 
32 
33  /** A class for storing a map of 3D probabilistic landmarks.
34  * <br>
35  * Currently these types of landmarks are defined: (see mrpt::maps::CLandmark)
36  * - For "visual landmarks" from images: features with associated descriptors.
37  * - For laser scanners: each of the range measuremnts, as "occupancy" landmarks.
38  * - For grid maps: "Panoramic descriptor" feature points.
39  * - For range-only localization and SLAM: Beacons. It is also supported the simulation of expected beacon-to-sensor readings, observation likelihood,...
40  * <br>
41  * <b>How to load landmarks from observations:</b><br>
42  * When invoking CLandmarksMap::insertObservation(), the values in CLandmarksMap::insertionOptions will
43  * determinate the kind of landmarks that will be extracted and fused into the map. Supported feature
44  * extraction processes are listed next:
45  *
46  <table>
47  <tr> <td><b>Observation class:</b></td> <td><b>Generated Landmarks:</b></td> <td><b>Comments:</b></td> </tr>
48  <tr> <td>CObservationImage</td> <td>vlSIFT</td> <td>1) A SIFT feature is created for each SIFT detected in the image,
49  <br>2) Correspondences between SIFTs features and existing ones are finded by computeMatchingWith3DLandmarks,
50  <br>3) The corresponding feaures are fused, and the new ones added, with an initial uncertainty according to insertionOptions</td> </tr>
51  <tr> <td>CObservationStereoImages</td> <td>vlSIFT</td> <td> Each image is separately procesed by the method for CObservationImage observations </td> </tr>
52  <tr> <td>CObservationStereoImages</td> <td>vlColor</td> <td> TODO... </td> </tr>
53  <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>
54  </table>
55  *
56  * \sa CMetricMap
57  * \ingroup mrpt_vision_grp
58  */
60  {
61  // This must be added to any CSerializable derived class:
63 
64  private:
66  bool internal_insertObservation( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose = NULL ) MRPT_OVERRIDE;
67 
68  public:
69  /** Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the world being modeled with this map.
70  *
71  * In the current implementation, this method behaves in a different way according to the nature of
72  * the observation's class:
73  * - "mrpt::obs::CObservation2DRangeScan": This calls "computeLikelihood_RSLC_2007".
74  * - "mrpt::obs::CObservationStereoImages": This calls "computeLikelihood_SIFT_LandmarkMap".
75  *
76  * \param takenFrom The robot's pose the observation is supposed to be taken from.
77  * \param obs The observation.
78  * \return This method returns a likelihood value > 0.
79  *
80  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
81  */
82  double internal_computeObservationLikelihood( const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom ) MRPT_OVERRIDE;
83 
84 
85  static mrpt::utils::TColorf COLOR_LANDMARKS_IN_3DSCENES; //!< The color of landmark ellipsoids in CLandmarksMap::getAs3DObject
86 
87  typedef mrpt::maps::CLandmark landmark_type;
88 
89 
90  /** The list of landmarks: the wrapper class is just for maintaining the KD-Tree representation
91  */
93  {
94  private:
95  /** The actual list */
97 
98  /** A grid-map with the set of landmarks falling into each cell.
99  * \todo Use the KD-tree instead?
100  */
102 
103  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
104  * \sa getLargestDistanceFromOrigin
105  */
107 
108  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
109  * \sa getLargestDistanceFromOrigin
110  */
112 
113  public:
114  /** Default constructor
115  */
117 
119  inline iterator begin() { return m_landmarks.begin(); };
120  inline iterator end() { return m_landmarks.end(); };
121  void clear();
122  inline size_t size() const { return m_landmarks.size(); };
123 
125  inline const_iterator begin() const { return m_landmarks.begin(); };
126  inline const_iterator end() const { return m_landmarks.end(); };
127 
128  /** The object is copied, thus the original copy passed as a parameter can be released.
129  */
130  void push_back( const CLandmark &lm);
131  CLandmark* get(unsigned int indx);
132  const CLandmark* get(unsigned int indx) const;
133  void isToBeModified(unsigned int indx);
134  void hasBeenModified(unsigned int indx);
136  void erase(unsigned int indx);
137 
139 
140  /** Returns the landmark with a given landmrk ID, or NULL if not found
141  */
143 
144  /** Returns the landmark with a given beacon ID, or NULL if not found
145  */
146  const CLandmark* getByBeaconID( unsigned int ID ) const;
147 
148  /** 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).
149  */
151 
152  } landmarks;
153 
154  /** Constructor
155  */
157 
158  /** Virtual destructor.
159  */
160  virtual ~CLandmarksMap();
161 
162 
163  /**** FAMD ***/
164  /** Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks
165  */
166  static std::map<std::pair<mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID>, double> _mEDD;
168  static bool _maxIDUpdated;
169 
171  /**** END FAMD *****/
172 
173  // See docs in base class
174  float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE;
175 
176  /** With this struct options are provided to the observation insertion process.
177  */
179  {
180  public:
181  /** Initilization of default parameters
182  */
184 
185  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
186  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
187 
188  /** If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D features.
189  */
191 
192  /** If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D features.
193  */
195 
196  /** If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for each range.
197  */
199 
200  /** [For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as correspondence (Default=0.4)
201  */
203 
204  /** [For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0.5)
205  */
207 
208  /****************************************** FAMD ******************************************/
209  /** [For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as correspondence (Default=200)
210  */
212 
213  /** [For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method))
214  * 0: Our method -> Euclidean Distance between Descriptors and 3D position
215  * 1: Sim, Elinas, Griffin, Little -> Euclidean Distance between Descriptors
216  */
217  unsigned int SIFTMatching3DMethod;
218 
219  /** [For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method))
220  * 0: Our method -> Euclidean Distance between Descriptors and 3D position
221  * 1: Sim, Elinas, Griffin, Little -> 3D position
222  */
223  unsigned int SIFTLikelihoodMethod;
224 
225  /****************************************** END FAMD ******************************************/
226 
227  /** [For SIFT landmarks only] The distance (in meters) of the mean value of landmarks, for the initial position PDF (Default = 3m)
228  */
230 
231  /** [For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perpendicular to the main directiom (Default = 0.05f)
232  */
234 
235  /** [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)
236  */
237  float SIFTs_stdXY, SIFTs_stdDisparity;
238 
239  /** Number of points to extract in the image
240  */
242 
243  /** Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation.
244  */
246 
247  /** Maximum distance (in pixels) from a point to a certain epipolar line to be considered a potential match.
248  */
250 
251  /** Indicates if the images (as well as the SIFT detected features) should be shown in a window.
252  */
254 
255  /** Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map.
256  * \note There exists another \a SIFT_feat_options field in the \a likelihoodOptions member.
257  * \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.
258  */
260 
261  } insertionOptions;
262 
263  /** With this struct options are provided to the likelihood computations.
264  */
266  {
267  public:
269 
270  void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source,const std::string &section) MRPT_OVERRIDE; // See base docs
271  void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE; // See base docs
272 
273  /** @name Parameters for: 2D LIDAR scans
274  * @{ */
275  unsigned int rangeScan2D_decimation; //!< The number of rays from a 2D range scan will be decimated by this factor (default = 1, no decimation)
276  /** @} */
277 
278  /** @name Parameters for: images
279  * @{ */
284  int SIFTs_decimation; //!< Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood computation.
285  /** Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map.
286  * \note There exists another \a SIFT_feat_options field in the \a insertionOptions member.
287  * \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. */
289  /** @} */
290 
291  /** @name Parameters for: Range-only observation
292  * @{ */
293  float beaconRangesStd; //!< The standard deviation used for Beacon ranges likelihood (default=0.08) [meters] \sa beaconRangesUseObservationStd
294  bool beaconRangesUseObservationStd; //!< (Default: false) If true, `beaconRangesStd` is ignored and each individual `CObservationBeaconRanges::stdError` field is used instead.
295  /** @} */
296 
297  /** @name Parameters for: GPS readings
298  * @{ */
299 
300  /** This struct store de GPS longitude, latitude (in degrees ) and altitude (in meters) for the first GPS observation
301  * compose with de sensor position on the robot */
303  {
304  public:
306  double longitude; //!< degrees
307  double latitude; //!< degrees
308  double altitude; //!< meters
309  /** These 3 params allow rotating and shifting GPS coordinates with other 2D maps (e.g. gridmaps).
310  * - ang : Map rotation [deg]
311  * - x_shift, y_shift: (x,y) offset [m] */
312  double ang, x_shift, y_shift;
313  unsigned int min_sat; //!< Minimum number of sats to take into account the data
314  } GPSOrigin;
315 
316  float GPS_sigma; //!< A constant "sigma" for GPS localization data (in meters)
317  /** @} */
318 
319  } likelihoodOptions;
320 
321  /** This struct stores extra results from invoking insertObservation
322  */
324  {
325  /** The number of SIFT detected in left and right images respectively
326  */
327 
328  unsigned int nSiftL, nSiftR;
329 
330 
331  } insertionResults;
332 
333  /** With this struct options are provided to the fusion process.
334  */
336  {
337  /** Initialization
338  */
340 
341  /** Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds.
342  */
343  unsigned int minTimesSeen;
344 
345  /** See "minTimesSeen"
346  */
348 
349  } fuseOptions;
350 
351 
352  /** Save to a text file.
353  * 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
354  *
355  * Returns false if any error occured, true elsewere.
356  */
357  bool saveToTextFile(std::string file);
358 
359  /** Save to a MATLAB script which displays 2D error ellipses for the map (top-view, projection on the XY plane).
360  * \param file The name of the file to save the script to.
361  * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
362  * \param stdCount The ellipsoids will be drawn from the center to "stdCount" times the "standard deviations". (default is 2std = 95% confidence intervals)
363  *
364  * \return Returns false if any error occured, true elsewere.
365  */
367  std::string file,
368  const char *style="b",
369  float stdCount = 2.0f );
370 
371  /** Save to a MATLAB script which displays 3D error ellipses for the map.
372  * \param file The name of the file to save the script to.
373  * \param style The MATLAB-like string for the style of the lines (see 'help plot' in MATLAB for posibilities)
374  * \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)
375  *
376  * \return Returns false if any error occured, true elsewere.
377  */
379  std::string file,
380  const char *style="b",
381  float confInterval = 0.95f ) const ;
382 
383  /** Returns the stored landmarks count.
384  */
385  size_t size() const;
386 
387  /** Computes the (logarithmic) likelihood function for a sensed observation "o" according to "this" map.
388  * This is the implementation of the algorithm reported in the paper:
389  <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>
390  */
391  double computeLikelihood_RSLC_2007( const CLandmarksMap *s, const mrpt::poses::CPose2D &sensorPose);
392 
393  /** Loads into this landmarks map the SIFT features extracted from an image observation (Previous contents of map will be erased)
394  * The robot is assumed to be at the origin of the map.
395  * Some options may be applicable from "insertionOptions" (insertionOptions.SIFTsLoadDistanceOfTheMean)
396  *
397  * \param feat_options Optionally, you can pass here parameters for changing the default SIFT detector settings.
398  */
400  const mrpt::obs::CObservationImage &obs,
402  );
403 
404  /** 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)
405  * The robot is assumed to be at the origin of the map.
406  * Some options may be applicable from "insertionOptions"
407  *
408  * \param feat_options Optionally, you can pass here parameters for changing the default SIFT detector settings.
409  */
414  );
415 
416  /** Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous contents of map will be erased)
417  * \param obs The observation
418  * \param robotPose The robot pose in the map (Default = the origin)
419  * Some options may be applicable from "insertionOptions"
420  */
423  const mrpt::poses::CPose3D *robotPose = NULL,
424  unsigned int downSampleFactor = 1);
425 
426  // See docs in base class
428  const mrpt::maps::CMetricMap *otherMap,
429  const mrpt::poses::CPose2D &otherMapPose,
430  float maxDistForCorrespondence,
431  float maxAngularDistForCorrespondence,
432  const mrpt::poses::CPose2D &angularDistPivotPoint,
433  mrpt::utils::TMatchingPairList &correspondences,
434  float &correspondencesRatio,
435  float *sumSqrDist = NULL,
436  bool onlyKeepTheClosest = false,
437  bool onlyUniqueRobust = false ) const;
438 
439  /** Perform a search for correspondences between "this" and another lansmarks map:
440  * In this class, the matching is established solely based on the landmarks' IDs.
441  * \param otherMap [IN] The other map.
442  * \param correspondences [OUT] The matched pairs between maps.
443  * \param correspondencesRatio [OUT] This is NumberOfMatchings / NumberOfLandmarksInTheAnotherMap
444  * \param otherCorrespondences [OUT] Will be returned with a vector containing "true" for the indexes of the other map's landmarks with a correspondence.
445  */
447  const mrpt::maps::CLandmarksMap *otherMap,
448  mrpt::utils::TMatchingPairList &correspondences,
449  float &correspondencesRatio,
450  std::vector<bool> &otherCorrespondences) const;
451 
452  /** Changes the reference system of the map to a given 3D pose.
453  */
455 
456  /** Changes the reference system of the map "otherMap" and save the result in "this" map.
457  */
459 
460  /** Fuses the contents of another map with this one, updating "this" object with the result.
461  * This process involves fusing corresponding landmarks, then adding the new ones.
462  * \param other The other landmarkmap, whose landmarks will be inserted into "this"
463  * \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...)
464  */
465  void fuseWith( CLandmarksMap &other, bool justInsertAllOfThem = false );
466 
467  /** Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
468  * See paper: JJAA 2006
469  */
471  CLandmarksMap *map,
472  mrpt::utils::TMatchingPairList *correspondences = NULL,
473  std::vector<bool> *otherCorrespondences = NULL);
474 
475  /** Returns true if the map is empty/no observation has been inserted.
476  */
477  bool isEmpty() const MRPT_OVERRIDE;
478 
479  /** Simulates a noisy reading toward each of the beacons in the landmarks map, if any.
480  * \param in_robotPose This robot pose is used to simulate the ranges to each beacon.
481  * \param in_sensorLocationOnRobot The 3D position of the sensor on the robot
482  * \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.
483  * 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.
484  */
485  void simulateBeaconReadings(
486  const mrpt::poses::CPose3D &in_robotPose,
487  const mrpt::poses::CPoint3D &in_sensorLocationOnRobot,
488  mrpt::obs::CObservationBeaconRanges &out_Observations ) const;
489 
490  /** Simulates a noisy bearing-range observation of all the beacons (landamrks with type glBeacon) in the landmarks map, if any.
491  * \param[in] robotPose The robot pose.
492  * \param[in] sensorLocationOnRobot The 3D position of the sensor on the robot
493  * \param[out] observations The results will be stored here.
494  * \param[in] sensorDetectsIDs If this is set to false, all the landmarks will be sensed with INVALID_LANDMARK_ID as ID.
495  * \param[in] stdRange The sigma of the sensor noise in range (meters).
496  * \param[in] stdYaw The sigma of the sensor noise in yaw (radians).
497  * \param[in] stdPitch The sigma of the sensor noise in pitch (radians).
498  * \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.
499  * \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.
500  * \param[in] spurious_count_std Read spurious_count_mean above.
501  *
502  * \note The fields "CObservationBearingRange::fieldOfView_*","CObservationBearingRange::maxSensorDistance" and "CObservationBearingRange::minSensorDistance" MUST BE FILLED OUT before calling this function.
503  * \note At output, the observation will have CObservationBearingRange::validCovariances set to "false" and the 3 sensor_std_* members correctly set to their values.
504  * 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-
505  */
506  void simulateRangeBearingReadings(
507  const mrpt::poses::CPose3D &robotPose,
508  const mrpt::poses::CPose3D &sensorLocationOnRobot,
509  mrpt::obs::CObservationBearingRange &observations,
510  bool sensorDetectsIDs = true,
511  const float stdRange = 0.01f,
512  const float stdYaw = mrpt::utils::DEG2RAD(0.1f),
513  const float stdPitch = mrpt::utils::DEG2RAD(0.1f),
514  vector_size_t *real_associations = NULL,
515  const double spurious_count_mean = 0,
516  const double spurious_count_std = 0
517  ) const;
518 
519 
520  /** 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).
521  * In the case of this class, these files are generated:
522  * - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks as 3D ellipses.
523  * - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane grid" and the set of ellipsoids in 3D.
524  */
525  void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE;
526 
527  /** Returns a 3D object representing the map.
528  * \sa COLOR_LANDMARKS_IN_3DSCENES
529  */
530  void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE;
531 
532  // See base docs
533  virtual void auxParticleFilterCleanUp() MRPT_OVERRIDE;
534 
536  typedef std::pair<mrpt::math::TPoint3D,unsigned int> TPairIdBeacon;
537  std::deque<TPairIdBeacon> initialBeacons; //!< Initial contents of the map, especified by a set of 3D Beacons with associated IDs
538  mrpt::maps::CLandmarksMap::TInsertionOptions insertionOpts;
539  mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOpts;
541 
542  }; // End of class def.
544 
545 
546  } // End of namespace
547 } // End of namespace
548 
549 #endif
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#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...
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ...
#define DEG2RAD
Definition: bits.h:86
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
int64_t TLandmarkID
The type for the IDs of landmarks.
A class for storing a map of 3D probabilistic landmarks.
double computeLikelihood_RSLC_2007(const CLandmarksMap *s, const mrpt::poses::CPose2D &sensorPose)
Computes the (logarithmic) likelihood function for a sensed observation "o" according to "this" map.
void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map,...
void computeMatchingWith3DLandmarks(const mrpt::maps::CLandmarksMap *otherMap, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: In this class,...
void loadSiftFeaturesFromStereoImageObservation(const mrpt::obs::CObservationStereoImages &obs, mrpt::maps::CLandmark::TLandmarkID fID, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of...
size_t size() const
Returns the stored landmarks count.
bool saveToTextFile(std::string file)
Save to a text file.
void loadSiftFeaturesFromImageObservation(const mrpt::obs::CObservationImage &obs, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an image observation (Previous content...
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg, const mrpt::maps::CLandmarksMap *otherMap)
Changes the reference system of the map "otherMap" and save the result in "this" map.
static mrpt::maps::CLandmark::TLandmarkID _mapMaxID
bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
void computeMatchingWith2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const mrpt::poses::CPose2D &angularDistPivotPoint, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=NULL, bool onlyKeepTheClosest=false, bool onlyUniqueRobust=false) const
double computeLikelihood_SIFT_LandmarkMap(CLandmarksMap *map, mrpt::utils::TMatchingPairList *correspondences=NULL, std::vector< bool > *otherCorrespondences=NULL)
Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
bool saveToMATLABScript2D(std::string file, const char *style="b", float stdCount=2.0f)
Save to a MATLAB script which displays 2D error ellipses for the map (top-view, projection on the XY ...
CLandmarksMap()
Constructor.
virtual ~CLandmarksMap()
Virtual destructor.
mrpt::maps::CLandmark::TLandmarkID getMapMaxID()
bool saveToMATLABScript3D(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.
static std::map< std::pair< mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID >, double > _mEDD
Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks.
void fuseWith(CLandmarksMap &other, bool justInsertAllOfThem=false)
Fuses the contents of another map with this one, updating "this" object with the result.
void loadOccupancyFeaturesFrom2DRangeScan(const mrpt::obs::CObservation2DRangeScan &obs, const mrpt::poses::CPose3D *robotPose=NULL, unsigned int downSampleFactor=1)
Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous cont...
Declares a virtual base class for all metric maps storage classes.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Declares a class derived from "CObservation" that encapsules an image from a camera,...
Observation class for either a pair of left+right or left+disparity images from a stereo camera.
A class used to store a 2D pose.
Definition: CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:39
A list of TMatchingPair.
Definition: TMatchingPair.h:67
Scalar * iterator
Definition: eigen_plugins.h:23
const Scalar * const_iterator
Definition: eigen_plugins.h:24
std::vector< size_t > vector_size_t
Definition: types_simple.h:25
@ featSIFT
Scale Invariant Feature Transform [LOWE'04].
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
std::vector< CLandmark > TSequenceLandmarks
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The list of landmarks: the wrapper class is just for maintaining the KD-Tree representation.
mrpt::utils::CDynamicGrid< vector_int > m_grid
A grid-map with the set of landmarks falling into each cell.
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
const CLandmark * getByBeaconID(unsigned int ID) const
Returns the landmark with a given beacon ID, or NULL if not found.
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
internal::TSequenceLandmarks m_landmarks
The actual list.
const CLandmark * get(unsigned int indx) const
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
internal::TSequenceLandmarks::iterator iterator
mrpt::utils::CDynamicGrid< vector_int > * getGrid()
const CLandmark * getByID(CLandmark::TLandmarkID ID) const
Returns the landmark with a given landmrk ID, or NULL if not found.
internal::TSequenceLandmarks::const_iterator const_iterator
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
With this struct options are provided to the fusion process.
unsigned int minTimesSeen
Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds.
With this struct options are provided to the observation insertion process.
float SiftCorrRatioThreshold
[For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as c...
bool PLOT_IMAGES
Indicates if the images (as well as the SIFT detected features) should be shown in a window.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
float SIFTs_epipolar_TH
Maximum distance (in pixels) from a point to a certain epipolar line to be considered a potential mat...
TInsertionOptions()
Initilization of default parameters.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
float SiftLikelihoodThreshold
[For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0...
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 SIFTsLoadEllipsoidWidth
[For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perp...
float SIFTs_stereo_maxDepth
Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation.
bool insert_Landmarks_from_range_scans
If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for ...
unsigned int SIFTMatching3DMethod
[For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method)) 0: Our method -> E...
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map.
unsigned int SIFTLikelihoodMethod
[For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 0: Our method -...
int SIFTs_numberOfKLTKeypoints
Number of points to extract in the image.
float SiftEDDThreshold
[For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as corres...
float SIFTsLoadDistanceOfTheMean
[For SIFT landmarks only] The distance (in meters) of the mean value of landmarks,...
bool insert_SIFTs_from_stereo_images
If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D ...
This struct stores extra results from invoking insertObservation.
unsigned int nSiftL
The number of SIFT detected in left and right images respectively.
This struct store de GPS longitude, latitude (in degrees ) and altitude (in meters) for the first GPS...
double ang
These 3 params allow rotating and shifting GPS coordinates with other 2D maps (e.g.
unsigned int min_sat
Minimum number of sats to take into account the data.
With this struct options are provided to the likelihood computations.
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map.
bool beaconRangesUseObservationStd
(Default: false) If true, beaconRangesStd is ignored and each individual CObservationBeaconRanges::st...
float beaconRangesStd
The standard deviation used for Beacon ranges likelihood (default=0.08) [meters].
unsigned int rangeScan2D_decimation
The number of rays from a 2D range scan will be decimated by this factor (default = 1,...
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
int SIFTs_decimation
Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood compu...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
float GPS_sigma
A constant "sigma" for GPS localization data (in meters)
std::pair< mrpt::math::TPoint3D, unsigned int > TPairIdBeacon
Parameters for CMetricMap::compute3DMatchingRatio()
The set of parameters for all the detectors & descriptor algorithms.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 03:44:04 UTC 2022