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