MRPT  2.0.1
CLandmarksMap.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
13 #include <mrpt/maps/CLandmark.h>
14 #include <mrpt/maps/CMetricMap.h>
15 #include <mrpt/math/CMatrixF.h>
20 #include <mrpt/obs/obs_frwds.h>
23 
24 namespace mrpt
25 {
26 namespace maps
27 {
28 namespace internal
29 {
30 using TSequenceLandmarks = std::vector<CLandmark>;
31 }
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
37  descriptors.
38  * - For laser scanners: each of the range measuremnts, as "occupancy"
39  landmarks.
40  * - For grid maps: "Panoramic descriptor" feature points.
41  * - For range-only localization and SLAM: Beacons. It is also supported
42  the simulation of expected beacon-to-sensor readings, observation
43  likelihood,...
44  * <br>
45  * <b>How to load landmarks from observations:</b><br>
46  * When invoking CLandmarksMap::insertObservation(), the values in
47  CLandmarksMap::insertionOptions will
48  * determinate the kind of landmarks that will be extracted and fused into
49  the map. Supported feature
50  * extraction processes are listed next:
51  *
52  <table>
53  <tr> <td><b>Observation class:</b></td> <td><b>Generated Landmarks:</b></td>
54  <td><b>Comments:</b></td> </tr>
55  <tr> <td>CObservationImage</td> <td>vlSIFT</td> <td>1) A SIFT feature is
56  created for each SIFT detected in the image,
57  <br>2) Correspondences between SIFTs features and existing ones are
58  finded by computeMatchingWith3DLandmarks,
59  <br>3) The corresponding feaures are fused, and the new ones added,
60  with an initial uncertainty according to insertionOptions</td> </tr>
61  <tr> <td>CObservationStereoImages</td> <td>vlSIFT</td> <td> Each image is
62  separately procesed by the method for CObservationImage observations </td>
63  </tr>
64  <tr> <td>CObservationStereoImages</td> <td>vlColor</td> <td> TODO... </td>
65  </tr>
66  <tr> <td>CObservation2DRangeScan</td> <td>glOccupancy</td> <td> A landmark is
67  added for each range in the scan, with its appropiate covariance matrix derived
68  from the jacobians matrixes. </td> </tr>
69  </table>
70  *
71  * \sa CMetricMap
72  * \ingroup mrpt_vision_grp
73  */
75 {
77 
78  private:
79  void internal_clear() override;
81  const mrpt::obs::CObservation& obs,
82  const mrpt::poses::CPose3D* robotPose = nullptr) override;
83 
84  public:
85  /** Computes the (logarithmic) likelihood that a given observation was taken
86  *from a given pose in the world being modeled with this map.
87  *
88  * In the current implementation, this method behaves in a different way
89  *according to the nature of
90  * the observation's class:
91  * - "mrpt::obs::CObservation2DRangeScan": This calls
92  *"computeLikelihood_RSLC_2007".
93  * - "mrpt::obs::CObservationStereoImages": This calls
94  *"computeLikelihood_SIFT_LandmarkMap".
95  * <table>
96  * <tr> <td><b>Observation class:</b></td> <td><b>Generated
97  *Landmarks:</b></td> <td><b>Comments:</b></td> </tr>
98  * <tr> <td>CObservationImage</td> <td>vlSIFT</td> <td>1) A SIFT feature is
99  *created for each SIFT detected in the image,
100  * <br>2) Correspondences between SIFTs features and existing ones
101  *are found by computeMatchingWith3DLandmarks,
102  * <br>3) The corresponding feaures are fused, and the new ones
103  * added,
104  *with an initial uncertainty according to insertionOptions</td> </tr>
105  * <tr> <td>CObservationStereoImages</td> <td>vlSIFT</td> <td> Each image is
106  *separately procesed by the method for CObservationImage observations </td>
107  *</tr>
108  * <tr> <td>CObservationStereoImages</td> <td>vlColor</td> <td> TODO...
109  *</td> </tr>
110  * <tr> <td>CObservation2DRangeScan</td> <td>glOccupancy</td> <td> A
111  *landmark is added for each range in the scan, with its appropiate
112  *covariance matrix derived from the jacobians matrixes. </td> </tr>
113  * </table>
114  *
115  * \param takenFrom The robot's pose the observation is supposed to be taken
116  *from.
117  * \param obs The observation.
118  * \return This method returns a likelihood value > 0.
119  *
120  * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
121  */
123  const mrpt::obs::CObservation& obs,
124  const mrpt::poses::CPose3D& takenFrom) override;
125 
126  /** The color of landmark ellipsoids in CLandmarksMap::getAs3DObject */
128 
130 
131  /** The list of landmarks: the wrapper class is just for maintaining the
132  * KD-Tree representation
133  */
135  {
136  private:
137  /** The actual list */
139 
140  /** A grid-map with the set of landmarks falling into each cell.
141  */
143 
144  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
145  * \sa getLargestDistanceFromOrigin
146  */
148 
149  /** Auxiliary variables used in "getLargestDistanceFromOrigin"
150  * \sa getLargestDistanceFromOrigin
151  */
153 
154  public:
155  /** Default constructor
156  */
158 
159  using iterator = internal::TSequenceLandmarks::iterator;
160  inline iterator begin() { return m_landmarks.begin(); };
161  inline iterator end() { return m_landmarks.end(); };
162  void clear();
163  inline size_t size() const { return m_landmarks.size(); };
164  using const_iterator = internal::TSequenceLandmarks::const_iterator;
165  inline const_iterator begin() const { return m_landmarks.begin(); };
166  inline const_iterator end() const { return m_landmarks.end(); };
167  /** The object is copied, thus the original copy passed as a parameter
168  * can be released.
169  */
170  void push_back(const CLandmark& lm);
171  CLandmark* get(unsigned int indx);
172  const CLandmark* get(unsigned int indx) const;
173  void isToBeModified(unsigned int indx);
174  void hasBeenModified(unsigned int indx);
175  void hasBeenModifiedAll();
176  void erase(unsigned int indx);
177 
179  {
180  return &m_grid;
181  }
182  /** Returns the landmark with a given landmrk ID, or nullptr if not
183  * found
184  */
185  const CLandmark* getByID(CLandmark::TLandmarkID ID) const;
186 
187  /** Returns the landmark with a given beacon ID, or nullptr if not found
188  */
189  const CLandmark* getByBeaconID(unsigned int ID) const;
190 
191  /** This method returns the largest distance from the origin to any of
192  * the points, such as a sphere centered at the origin with this radius
193  * cover ALL the points in the map (the results are buffered, such as,
194  * if the map is not modified, the second call will be much faster than
195  * the first one).
196  */
197  float getLargestDistanceFromOrigin() const;
198 
199  } landmarks;
200 
201  CLandmarksMap() = default;
202  ~CLandmarksMap() override = default;
203 
204  /**** FAMD ***/
205  /** Map of the Euclidean Distance between the descriptors of two SIFT-based
206  * landmarks
207  */
208  static std::map<
209  std::pair<
212  double>
215  static bool _maxIDUpdated;
216 
218  /**** END FAMD *****/
219 
220  // See docs in base class
222  const mrpt::maps::CMetricMap* otherMap,
223  const mrpt::poses::CPose3D& otherMapPose,
224  const TMatchingRatioParams& params) const override;
225 
226  /** With this struct options are provided to the observation insertion
227  * process.
228  */
230  {
231  public:
232  /** Initilization of default parameters
233  */
235 
236  void loadFromConfigFile(
237  const mrpt::config::CConfigFileBase& source,
238  const std::string& section) override; // See base docs
239  void dumpToTextStream(
240  std::ostream& out) const override; // See base docs
241 
242  /** If set to true (default), the insertion of a CObservationImage in
243  * the map will insert SIFT 3D features.
244  */
246 
247  /** If set to true (default), the insertion of a
248  * CObservationStereoImages in the map will insert SIFT 3D features.
249  */
251 
252  /** If set to true (default), inserting a CObservation2DRangeScan in the
253  * map will generate landmarks for each range.
254  */
256 
257  /** [For SIFT landmarks only] The ratio between the best and second best
258  * descriptor distances to set as correspondence (Default=0.4)
259  */
261 
262  /** [For SIFT landmarks only] The minimum likelihood value of a match to
263  * set as correspondence (Default=0.5)
264  */
266 
267  /****************************************** FAMD
268  * ******************************************/
269  /** [For SIFT landmarks only] The minimum Euclidean Descriptor Distance
270  * value of a match to set as correspondence (Default=200)
271  */
272  float SiftEDDThreshold{200.0f};
273 
274  /** [For SIFT landmarks only] Method to compute 3D matching (Default = 0
275  * (Our method))
276  * 0: Our method -> Euclidean Distance between Descriptors and 3D
277  * position
278  * 1: Sim, Elinas, Griffin, Little -> Euclidean Distance between
279  * Descriptors
280  */
281  unsigned int SIFTMatching3DMethod{0};
282 
283  /** [For SIFT landmarks only] Method to compute the likelihood (Default
284  * = 0 (Our method))
285  * 0: Our method -> Euclidean Distance between Descriptors and 3D
286  * position
287  * 1: Sim, Elinas, Griffin, Little -> 3D position
288  */
289  unsigned int SIFTLikelihoodMethod{0};
290 
291  /****************************************** END FAMD
292  * ******************************************/
293 
294  /** [For SIFT landmarks only] The distance (in meters) of the mean value
295  * of landmarks, for the initial position PDF (Default = 3m)
296  */
298 
299  /** [For SIFT landmarks only] The width (in meters, standard deviation)
300  * of the ellipsoid in the axis perpendicular to the main directiom
301  * (Default = 0.05f)
302  */
304 
305  /** [For SIFT landmarks only] The standard deviation (in pixels) for the
306  * SIFTs detector (This is used for the Jacobbian to project stereo
307  * images to 3D)
308  */
309  float SIFTs_stdXY{2.0f}, SIFTs_stdDisparity{1.0f};
310 
311  /** Number of points to extract in the image
312  */
314 
315  /** Maximum depth of 3D landmarks when loading a landmarks map from a
316  * stereo image observation.
317  */
318  float SIFTs_stereo_maxDepth{15.0f};
319 
320  /** Maximum distance (in pixels) from a point to a certain epipolar line
321  * to be considered a potential match.
322  */
323  float SIFTs_epipolar_TH{1.5f};
324 
325  /** Indicates if the images (as well as the SIFT detected features)
326  * should be shown in a window.
327  */
328  bool PLOT_IMAGES{false};
329 
330  /** Parameters of the SIFT feature detector/descriptors while inserting
331  * images in the landmark map.
332  * \note There exists another \a SIFT_feat_options field in the \a
333  * likelihoodOptions member.
334  * \note All parameters of this field can be loaded from a config
335  * file. See mrpt::vision::CFeatureExtraction::TOptions for the names of
336  * the expected fields.
337  */
339 
341 
342  /** With this struct options are provided to the likelihood computations.
343  */
345  {
346  public:
348 
349  void loadFromConfigFile(
350  const mrpt::config::CConfigFileBase& source,
351  const std::string& section) override; // See base docs
352  void dumpToTextStream(
353  std::ostream& out) const override; // See base docs
354 
355  /** @name Parameters for: 2D LIDAR scans
356  * @{ */
357  /** The number of rays from a 2D range scan will be decimated by this
358  * factor (default = 1, no decimation) */
359  unsigned int rangeScan2D_decimation{20};
360  /** @} */
361 
362  /** @name Parameters for: images
363  * @{ */
366  float SIFTs_mahaDist_std{4.0f};
368  /** Considers 1 out of "SIFTs_decimation" visual landmarks in the
369  * observation during the likelihood computation. */
371  /** Parameters of the SIFT feature detector/descriptors while inserting
372  * images in the landmark map.
373  * \note There exists another \a SIFT_feat_options field in the \a
374  * insertionOptions member.
375  * \note All parameters of this field can be loaded from a config
376  * file. See mrpt::vision::CFeatureExtraction::TOptions for the names of
377  * the expected fields. */
379  /** @} */
380 
381  /** @name Parameters for: Range-only observation
382  * @{ */
383  /** The standard deviation used for Beacon ranges likelihood
384  * (default=0.08) [meters] \sa beaconRangesUseObservationStd */
385  float beaconRangesStd{0.08f};
386  /** (Default: false) If true, `beaconRangesStd` is ignored and each
387  * individual `CObservationBeaconRanges::stdError` field is used
388  * instead. */
390  /** @} */
391 
392  /** @name Parameters for: External robot poses observation
393  * @{ */
394  /** The standard deviation used for external robot poses likelihood
395  * (default=0.05) [meters] */
396  float extRobotPoseStd{0.05f};
397  /** @} */
398 
399  /** @name Parameters for: GPS readings
400  * @{ */
401 
402  /** This struct store de GPS longitude, latitude (in degrees ) and
403  * altitude (in meters) for the first GPS observation
404  * compose with de sensor position on the robot */
405  struct TGPSOrigin
406  {
407  public:
408  TGPSOrigin();
409  /** degrees */
410  double longitude{-4.47763833333333};
411  /** degrees */
412  double latitude{36.71559000000000};
413  /** meters */
414  double altitude{42.3};
415  /** These 3 params allow rotating and shifting GPS coordinates with
416  * other 2D maps (e.g. gridmaps).
417  * - ang : Map rotation [deg]
418  * - x_shift, y_shift: (x,y) offset [m] */
419  double ang{0}, x_shift{0}, y_shift{0};
420  /** Minimum number of sats to take into account the data */
421  unsigned int min_sat{4};
422  } GPSOrigin;
423 
424  /** A constant "sigma" for GPS localization data (in meters) */
425  float GPS_sigma{1.0f};
426  /** @} */
427 
429 
430  /** This struct stores extra results from invoking insertObservation
431  */
433  {
434  /** The number of SIFT detected in left and right images respectively
435  */
436 
437  unsigned int nSiftL, nSiftR;
438 
440 
441  /** With this struct options are provided to the fusion process.
442  */
444  {
445  /** Initialization
446  */
447  TFuseOptions();
448 
449  /** Required number of times of a landmark to be seen not to be removed,
450  * in "ellapsedTime" seconds.
451  */
452  unsigned int minTimesSeen{2};
453 
454  /** See "minTimesSeen"
455  */
456  float ellapsedTime{4.0f};
457 
458  } fuseOptions;
459 
460  /** Save to a text file.
461  * In line "i" there are the (x,y,z) mean values of the i'th landmark +
462  * type of landmark + # times seen + timestamp + RGB/descriptor + ID
463  *
464  * Returns false if any error occured, true elsewere.
465  */
466  bool saveToTextFile(std::string file);
467 
468  /** Save to a MATLAB script which displays 2D error ellipses for the map
469  *(top-view, projection on the XY plane).
470  * \param file The name of the file to save the script to.
471  * \param style The MATLAB-like string for the style of the lines (see
472  *'help plot' in MATLAB for possibilities)
473  * \param stdCount The ellipsoids will be drawn from the center to
474  *"stdCount" times the "standard deviations". (default is 2std = 95%
475  *confidence intervals)
476  *
477  * \return Returns false if any error occured, true elsewere.
478  */
480  std::string file, const char* style = "b", float stdCount = 2.0f);
481 
482  /** Save to a MATLAB script which displays 3D error ellipses for the map.
483  * \param file The name of the file to save the script to.
484  * \param style The MATLAB-like string for the style of the lines (see
485  *'help plot' in MATLAB for possibilities)
486  * \param stdCount The ellipsoids will be drawn from the center to a given
487  *confidence interval in [0,1], e.g. 2 sigmas=0.95 (default is 2std = 0.95
488  *confidence intervals)
489  *
490  * \return Returns false if any error occured, true elsewere.
491  */
493  std::string file, const char* style = "b",
494  float confInterval = 0.95f) const;
495 
496  /** Returns the stored landmarks count.
497  */
498  size_t size() const;
499 
500  /** Computes the (logarithmic) likelihood function for a sensed observation
501  "o" according to "this" map.
502  * This is the implementation of the algorithm reported in the paper:
503  <em>J.L. Blanco, J. Gonzalez, and J.A. Fernandez-Madrigal, "A
504  Consensus-based Approach for Estimating the Observation Likelihood of
505  Accurate Range Sensors", in IEEE International Conference on Robotics and
506  Automation (ICRA), Rome (Italy), Apr 10-14, 2007</em>
507  */
509  const CLandmarksMap* s, const mrpt::poses::CPose2D& sensorPose);
510 
511  /** Loads into this landmarks map the SIFT features extracted from an image
512  * observation (Previous contents of map will be erased)
513  * The robot is assumed to be at the origin of the map.
514  * Some options may be applicable from "insertionOptions"
515  * (insertionOptions.SIFTsLoadDistanceOfTheMean)
516  *
517  * \param feat_options Optionally, you can pass here parameters for
518  * changing the default SIFT detector settings.
519  */
521  const mrpt::obs::CObservationImage& obs,
522  const mrpt::vision::CFeatureExtraction::TOptions& feat_options =
524 
525  /** Loads into this landmarks map the SIFT features extracted from an
526  * observation consisting of a pair of stereo-image (Previous contents of
527  * map will be erased)
528  * The robot is assumed to be at the origin of the map.
529  * Some options may be applicable from "insertionOptions"
530  *
531  * \param feat_options Optionally, you can pass here parameters for
532  * changing the default SIFT detector settings.
533  */
537  const mrpt::vision::CFeatureExtraction::TOptions& feat_options =
539 
540  /** Loads into this landmarks-map a set of occupancy features according to a
541  * 2D range scan (Previous contents of map will be erased)
542  * \param obs The observation
543  * \param robotPose The robot pose in the map (Default = the origin)
544  * Some options may be applicable from "insertionOptions"
545  */
548  const mrpt::poses::CPose3D* robotPose = nullptr,
549  unsigned int downSampleFactor = 1);
550 
551  // See docs in base class
553  const mrpt::maps::CMetricMap* otherMap,
554  const mrpt::poses::CPose2D& otherMapPose,
555  float maxDistForCorrespondence, float maxAngularDistForCorrespondence,
556  const mrpt::poses::CPose2D& angularDistPivotPoint,
557  mrpt::tfest::TMatchingPairList& correspondences,
558  float& correspondencesRatio, float* sumSqrDist = nullptr,
559  bool onlyKeepTheClosest = false, bool onlyUniqueRobust = false) const;
560 
561  /** Perform a search for correspondences between "this" and another
562  * lansmarks map:
563  * In this class, the matching is established solely based on the
564  * landmarks' IDs.
565  * \param otherMap [IN] The other map.
566  * \param correspondences [OUT] The matched pairs between maps.
567  * \param correspondencesRatio [OUT] This is NumberOfMatchings /
568  * NumberOfLandmarksInTheAnotherMap
569  * \param otherCorrespondences [OUT] Will be returned with a vector
570  * containing "true" for the indexes of the other map's landmarks with a
571  * correspondence.
572  */
574  const mrpt::maps::CLandmarksMap* otherMap,
575  mrpt::tfest::TMatchingPairList& correspondences,
576  float& correspondencesRatio,
577  std::vector<bool>& otherCorrespondences) const;
578 
579  /** Changes the reference system of the map to a given 3D pose.
580  */
582 
583  /** Changes the reference system of the map "otherMap" and save the result
584  * in "this" map.
585  */
587  const mrpt::poses::CPose3D& newOrg,
588  const mrpt::maps::CLandmarksMap* otherMap);
589 
590  /** Fuses the contents of another map with this one, updating "this" object
591  * with the result.
592  * This process involves fusing corresponding landmarks, then adding the
593  * new ones.
594  * \param other The other landmarkmap, whose landmarks will be inserted
595  * into "this"
596  * \param justInsertAllOfThem If set to "true", all the landmarks in
597  * "other" will be inserted into "this" without checking for possible
598  * correspondences (may appear duplicates ones, etc...)
599  */
600  void fuseWith(CLandmarksMap& other, bool justInsertAllOfThem = false);
601 
602  /** Returns the (logarithmic) likelihood of a set of landmarks "map" given
603  * "this" map.
604  * See paper: JJAA 2006
605  */
607  CLandmarksMap* map,
608  mrpt::tfest::TMatchingPairList* correspondences = nullptr,
609  std::vector<bool>* otherCorrespondences = nullptr);
610 
611  /** Returns true if the map is empty/no observation has been inserted.
612  */
613  bool isEmpty() const override;
614 
615  /** Simulates a noisy reading toward each of the beacons in the landmarks
616  * map, if any.
617  * \param in_robotPose This robot pose is used to simulate the ranges to
618  * each beacon.
619  * \param in_sensorLocationOnRobot The 3D position of the sensor on the
620  * robot
621  * \param out_Observations The results will be stored here. NOTICE that the
622  * fields
623  * "CObservationBeaconRanges::minSensorDistance","CObservationBeaconRanges::maxSensorDistance"
624  * and "CObservationBeaconRanges::stdError" MUST BE FILLED OUT before
625  * calling this function.
626  * An observation will be generated for each beacon in the map, but notice
627  * that some of them may be missed if out of the sensor maximum range.
628  */
630  const mrpt::poses::CPose3D& in_robotPose,
631  const mrpt::poses::CPoint3D& in_sensorLocationOnRobot,
632  mrpt::obs::CObservationBeaconRanges& out_Observations) const;
633 
634  /** Simulates a noisy bearing-range observation of all the beacons
635  * (landamrks with type glBeacon) in the landmarks map, if any.
636  * \param[in] robotPose The robot pose.
637  * \param[in] sensorLocationOnRobot The 3D position of the sensor on the
638  * robot
639  * \param[out] observations The results will be stored here.
640  * \param[in] sensorDetectsIDs If this is set to false, all the landmarks
641  * will be sensed with INVALID_LANDMARK_ID as ID.
642  * \param[in] stdRange The sigma of the sensor noise in range (meters).
643  * \param[in] stdYaw The sigma of the sensor noise in yaw (radians).
644  * \param[in] stdPitch The sigma of the sensor noise in pitch (radians).
645  * \param[out] real_associations If it's not a nullptr pointer, this will
646  * contain at the return the real indices of the landmarks in the map in the
647  * same order than they appear in out_Observations. Useful when
648  * sensorDetectsIDs=false. Spurious readings are assigned a
649  * std::string::npos (=-1) index.
650  * \param[in] spurious_count_mean The mean number of spurious measurements
651  * (uniformly distributed in range & angle) to generate. The number of
652  * spurious is generated by rounding a random Gaussin number. If both this
653  * mean and the std are zero (the default) no spurious readings are
654  * generated.
655  * \param[in] spurious_count_std Read spurious_count_mean above.
656  *
657  * \note The fields
658  * "CObservationBearingRange::fieldOfView_*","CObservationBearingRange::maxSensorDistance"
659  * and "CObservationBearingRange::minSensorDistance" MUST BE FILLED OUT
660  * before calling this function.
661  * \note At output, the observation will have
662  * CObservationBearingRange::validCovariances set to "false" and the 3
663  * sensor_std_* members correctly set to their values.
664  * An observation will be generated for each beacon in the map, but notice
665  * that some of them may be missed if out of the sensor maximum range or
666  * field of view-
667  */
669  const mrpt::poses::CPose3D& robotPose,
670  const mrpt::poses::CPose3D& sensorLocationOnRobot,
672  bool sensorDetectsIDs = true, const float stdRange = 0.01f,
673  const float stdYaw = mrpt::DEG2RAD(0.1f),
674  const float stdPitch = mrpt::DEG2RAD(0.1f),
675  std::vector<size_t>* real_associations = nullptr,
676  const double spurious_count_mean = 0,
677  const double spurious_count_std = 0) const;
678 
679  /** This virtual method saves the map to a file "filNamePrefix"+<
680  *some_file_extension >, as an image or in any other applicable way (Notice
681  *that other methods to save the map may be implemented in classes
682  *implementing this virtual interface).
683  * In the case of this class, these files are generated:
684  * - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks
685  *as
686  *3D ellipses.
687  * - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane
688  *grid"
689  *and the set of ellipsoids in 3D.
690  */
692  const std::string& filNamePrefix) const override;
693 
694  /** Returns a 3D object representing the map.
695  * \sa COLOR_LANDMARKS_IN_3DSCENES
696  */
697  void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
698 
699  // See base docs
700  void auxParticleFilterCleanUp() override;
701 
703  using TPairIdBeacon = std::pair<mrpt::math::TPoint3D, unsigned int>;
704  /** Initial contents of the map, especified by a set of 3D Beacons with
705  * associated IDs */
706  std::deque<TPairIdBeacon> initialBeacons;
707  mrpt::maps::CLandmarksMap::TInsertionOptions insertionOpts;
708  mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOpts;
710 
711 }; // End of class def.
712 
713 } // namespace maps
714 } // namespace mrpt
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
internal::TSequenceLandmarks::iterator iterator
Parameters for CMetricMap::compute3DMatchingRatio()
float SiftEDDThreshold
[For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as corres...
Declares a class derived from "CObservation" that encapsules an image from a camera, whose relative pose to robot is also stored.
size_t size() const
Returns the stored landmarks count.
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:38
bool insert_SIFTs_from_stereo_images
If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D ...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
unsigned int SIFTLikelihoodMethod
[For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 0: Our method -...
~CLandmarksMap() override=default
float beaconRangesStd
The standard deviation used for Beacon ranges likelihood (default=0.08) [meters]. ...
const CLandmark * getByID(CLandmark::TLandmarkID ID) const
Returns the landmark with a given landmrk ID, or nullptr if not found.
float SIFTs_stereo_maxDepth
Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation.
void fuseWith(CLandmarksMap &other, bool justInsertAllOfThem=false)
Fuses the contents of another map with this one, updating "this" object with the result.
#define MAP_DEFINITION_START(_CLASS_NAME_)
Add a MAP_DEFINITION_START() ...
With this struct options are provided to the observation insertion process.
struct mrpt::maps::CLandmarksMap::TFuseOptions fuseOptions
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
float extRobotPoseStd
The standard deviation used for external robot poses likelihood (default=0.05) [meters].
mrpt::vision::TStereoCalibParams params
The set of parameters for all the detectors & descriptor algorithms.
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
Scale Invariant Feature Transform [LOWE&#39;04].
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
STL namespace.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::containers::CDynamicGrid< std::vector< int32_t > > m_grid
A grid-map with the set of landmarks falling into each cell.
This struct store de GPS longitude, latitude (in degrees ) and altitude (in meters) for the first GPS...
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
void simulateBeaconReadings(const mrpt::poses::CPose3D &in_robotPose, const mrpt::poses::CPoint3D &in_sensorLocationOnRobot, mrpt::obs::CObservationBeaconRanges &out_Observations) const
Simulates a noisy reading toward each of the beacons in the landmarks map, if any.
unsigned int nSiftL
The number of SIFT detected in left and right images respectively.
The list of landmarks: the wrapper class is just for maintaining the KD-Tree representation.
With this struct options are provided to the likelihood computations.
const CLandmark * getByBeaconID(unsigned int ID) const
Returns the landmark with a given beacon ID, or nullptr if not found.
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >...
void internal_clear() override
Internal method called by clear()
std::vector< CLandmark > TSequenceLandmarks
Definition: CLandmarksMap.h:30
bool beaconRangesUseObservationStd
(Default: false) If true, beaconRangesStd is ignored and each individual CObservationBeaconRanges::st...
static mrpt::maps::CLandmark::TLandmarkID _mapMaxID
int64_t TLandmarkID
The type for the IDs of landmarks.
Definition: CLandmark.h:41
bool insert_SIFTs_from_monocular_images
If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D feature...
mrpt::maps::CLandmark::TLandmarkID getMapMaxID()
float SIFTsLoadDistanceOfTheMean
[For SIFT landmarks only] The distance (in meters) of the mean value of landmarks, for the initial position PDF (Default = 3m)
int SIFTs_numberOfKLTKeypoints
Number of points to extract in the image.
This class allows loading and storing values and vectors of different types from a configuration text...
int SIFTs_decimation
Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood compu...
A class for storing a map of 3D probabilistic landmarks.
Definition: CLandmarksMap.h:74
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.
constexpr double DEG2RAD(const double x)
Degrees to radians.
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
float SIFTs_stdXY
[For SIFT landmarks only] The standard deviation (in pixels) for the SIFTs detector (This is used for...
std::pair< mrpt::math::TPoint3D, unsigned int > TPairIdBeacon
A list of TMatchingPair.
Definition: TMatchingPair.h:70
Observation class for either a pair of left+right or left+disparity images from a stereo camera...
static mrpt::img::TColorf COLOR_LANDMARKS_IN_3DSCENES
The color of landmark ellipsoids in CLandmarksMap::getAs3DObject.
double computeLikelihood_SIFT_LandmarkMap(CLandmarksMap *map, mrpt::tfest::TMatchingPairList *correspondences=nullptr, std::vector< bool > *otherCorrespondences=nullptr)
Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
TPoint3D_< double > TPoint3D
Lightweight 3D point.
Definition: TPoint3D.h:268
A class used to store a 3D point.
Definition: CPoint3D.h:31
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map...
mrpt::maps::CLandmarksMap::TInsertionOptions insertionOptions
unsigned int minTimesSeen
Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds...
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.
bool saveToTextFile(std::string file)
Save to a text file.
unsigned int rangeScan2D_decimation
The number of rays from a 2D range scan will be decimated by this factor (default = 1...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
Definition: CLandmark.h:35
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
float ellapsedTime
See "minTimesSeen".
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
bool PLOT_IMAGES
Indicates if the images (as well as the SIFT detected features) should be shown in a window...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
float SIFTsLoadEllipsoidWidth
[For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perp...
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...
mrpt::containers::CDynamicGrid< std::vector< int32_t > > * getGrid()
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
An RGBA color - floats in the range [0,1].
Definition: TColor.h:88
TInsertionOptions()
Initilization of default parameters.
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 computeMatchingWith2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const mrpt::poses::CPose2D &angularDistPivotPoint, mrpt::tfest::TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=nullptr, bool onlyKeepTheClosest=false, bool onlyUniqueRobust=false) const
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Computes the (logarithmic) likelihood that a given observation was taken from a given pose in the wor...
void auxParticleFilterCleanUp() override
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::...
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
bool insert_Landmarks_from_range_scans
If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for ...
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...
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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 ...
internal::TSequenceLandmarks::const_iterator const_iterator
void simulateRangeBearingReadings(const mrpt::poses::CPose3D &robotPose, const mrpt::poses::CPose3D &sensorLocationOnRobot, mrpt::obs::CObservationBearingRange &observations, bool sensorDetectsIDs=true, const float stdRange=0.01f, const float stdYaw=mrpt::DEG2RAD(0.1f), const float stdPitch=mrpt::DEG2RAD(0.1f), std::vector< size_t > *real_associations=nullptr, const double spurious_count_mean=0, const double spurious_count_std=0) const
Simulates a noisy bearing-range observation of all the beacons (landamrks with type glBeacon) in the ...
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.
struct mrpt::maps::CLandmarksMap::TLikelihoodOptions::TGPSOrigin GPSOrigin
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 SiftLikelihoodThreshold
[For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0...
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map...
#define MAP_DEFINITION_END(_CLASS_NAME_)
double ang
These 3 params allow rotating and shifting GPS coordinates with other 2D maps (e.g.
void computeMatchingWith3DLandmarks(const mrpt::maps::CLandmarksMap *otherMap, mrpt::tfest::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: In this class...
float SiftCorrRatioThreshold
[For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as c...
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted.
struct mrpt::maps::CLandmarksMap::TCustomSequenceLandmarks landmarks
void loadOccupancyFeaturesFrom2DRangeScan(const mrpt::obs::CObservation2DRangeScan &obs, const mrpt::poses::CPose3D *robotPose=nullptr, unsigned int downSampleFactor=1)
Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous cont...
unsigned int min_sat
Minimum number of sats to take into account the data.
float GPS_sigma
A constant "sigma" for GPS localization data (in meters)
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const override
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
With this struct options are provided to the fusion process.
struct mrpt::maps::CLandmarksMap::TInsertionResults insertionResults
mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOptions
unsigned int SIFTMatching3DMethod
[For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method)) 0: Our method -> E...



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020