MRPT  1.9.9
CBeacon.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 
11 #include <mrpt/math/CMatrixF.h>
12 #include <mrpt/obs/CObservation.h> // INVALID_BEACON_ID
13 #include <mrpt/poses/CPoint3D.h>
18 
20 
21 namespace mrpt::maps
22 {
23 class CBeaconMap;
24 
25 /** The class for storing individual "beacon landmarks" under a variety of 3D
26  * position PDF distributions.
27  * This class is used for storage within the class CBeaconMap.
28  * The class implements the same methods than the interface "CPointPDF", and
29  * invoking them actually becomes
30  * a mapping into the methods of the current PDF representation of the
31  * beacon, selectable by means of "m_typePDF"
32  * \sa CBeaconMap, CPointPDFSOG
33  * \ingroup mrpt_maps_grp
34  */
36 {
38 
39  public:
40  /** The type for the IDs of landmarks.
41  */
42  using TBeaconID = int64_t;
43 
44  /** See m_typePDF
45  */
46  enum TTypePDF
47  {
51  };
52 
53  /** Which one of the different 3D point PDF is currently used in this
54  * object: montecarlo, gaussian, or a sum of gaussians.
55  * \sa m_location
56  */
58 
59  /** The individual PDF, if m_typePDF=pdfMonteCarlo (publicly accesible for
60  * ease of use, but the CPointPDF interface is also implemented in CBeacon).
61  */
63  /** The individual PDF, if m_typePDF=pdfGauss (publicly accesible for ease
64  * of use, but the CPointPDF interface is also implemented in CBeacon). */
66  /** The individual PDF, if m_typePDF=pdfSOG (publicly accesible for ease of
67  * use, but the CPointPDF interface is also implemented in CBeacon). */
69 
70  /** An ID for the landmark (see details next...)
71  * This ID was introduced in the version 3 of this class (21/NOV/2006),
72  *and its aim is
73  * to provide a way for easily establishing correspondences between
74  *landmarks detected
75  * in sequential image frames. Thus, the management of this field should
76  *be:
77  * - In 'servers' (classes/modules/... that detect landmarks from
78  *images):
79  *A different ID must be assigned to every landmark (e.g. a sequential
80  *counter), BUT only in the case of being sure of the correspondence of one
81  *landmark with another one in the past (e.g. tracking).
82  * - In 'clients': This field can be ignored, but if it is used, the
83  *advantage is solving the correspondence between landmarks detected in
84  *consequentive instants of time: Two landmarks with the same ID
85  *<b>correspond</b> to the same physical feature, BUT it should not be
86  *expected the inverse to be always true.
87  *
88  * Note that this field is never fill out automatically, it must be set by
89  *the programmer if used.
90  */
92 
93  void getMean(mrpt::poses::CPoint3D& mean_point) const override;
94 
95  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override;
96 
97  /** Copy operator, translating if necesary (for example, between particles
98  * and gaussian representations) */
99  void copyFrom(const mrpt::poses::CPointPDF& o) override;
100 
101  /** Save PDF's particles to a text file. See derived classes for more
102  * information about the format of generated files */
103  bool saveToTextFile(const std::string& file) const override;
104 
105  /** this = p (+) this. This can be used to convert a PDF from local
106  * coordinates to global, providing the point (newReferenceBase) from which
107  * "to project" the current pdf. Result PDF substituted the currently
108  * stored one in the object.
109  */
111  const mrpt::poses::CPose3D& newReferenceBase) override;
112 
113  /** Saves a 3D representation of the beacon into a given OpenGL scene */
115 
116  /** Gets a set of MATLAB commands which draw the current state of the
117  * beacon: */
118  void getAsMatlabDrawCommands(std::vector<std::string>& out_Str) const;
119 
120  /** Draw a sample from the pdf. */
121  void drawSingleSample(mrpt::poses::CPoint3D& outSample) const override;
122 
123  /** Bayesian fusion of two point distributions (product of two
124  * distributions->new distribution), then save the result in this object
125  * (WARNING: See implementing classes to see classes that can and cannot be
126  * mixtured!)
127  * \param p1 The first distribution to fuse
128  * \param p2 The second distribution to fuse
129  * \param minMahalanobisDistToDrop If set to different of 0, the result of
130  * very separate Gaussian modes (that will result in negligible components)
131  * in SOGs will be dropped to reduce the number of modes in the output.
132  */
133  void bayesianFusion(
134  const CPointPDF& p1, const CPointPDF& p2,
135  const double minMahalanobisDistToDrop = 0) override;
136 
137  /** Compute the observation model p(z_t|x_t) for a given observation (range
138  * value), and return it as an approximate SOG.
139  * Note that if the beacon is a SOG itself, the number of gaussian modes
140  * will be square.
141  * As a speed-up, if a "center point"+"maxDistanceFromCenter" is supplied
142  * (maxDistanceFromCenter!=0), those modes farther than this sphere will be
143  * discarded.
144  * Parameters such as the stdSigma of the sensor are gathered from
145  * "myBeaconMap"
146  * The result is one "ring" for each Gaussian mode that represent the
147  * beacon position in this object.
148  * The position of the sensor on the robot is used to shift the resulting
149  * densities such as they represent the position of the robot, not the
150  * sensor.
151  * \sa CBeaconMap::insertionOptions, generateRingSOG
152  */
154  float sensedRange, mrpt::poses::CPointPDFSOG& outPDF,
155  const CBeaconMap* myBeaconMap,
156  const mrpt::poses::CPoint3D& sensorPntOnRobot,
157  const mrpt::poses::CPoint3D& centerPoint =
158  mrpt::poses::CPoint3D(0, 0, 0),
159  float maxDistanceFromCenter = 0) const;
160 
161  /** This static method returns a SOG with ring-shape (or as a 3D sphere)
162  * that can be used to initialize a beacon if observed the first time.
163  * sensorPnt is the center of the ring/sphere, i.e. the absolute position
164  * of the range sensor.
165  * If clearPreviousContentsOutPDF=false, the SOG modes will be added to
166  * the current contents of outPDF
167  * If the 3x3 matrix covarianceCompositionToAdd is provided, it will be
168  * add to every Gaussian (to model the composition of uncertainty).
169  * \sa generateObservationModelDistribution
170  */
171  static void generateRingSOG(
172  float sensedRange, mrpt::poses::CPointPDFSOG& outPDF,
173  const CBeaconMap* myBeaconMap, const mrpt::poses::CPoint3D& sensorPnt,
174  const mrpt::math::CMatrixDouble33* covarianceCompositionToAdd = nullptr,
175  bool clearPreviousContentsOutPDF = true,
176  const mrpt::poses::CPoint3D& centerPoint =
177  mrpt::poses::CPoint3D(0, 0, 0),
178  float maxDistanceFromCenter = 0);
179 
180 }; // End of class definition
181 
182 } // namespace mrpt::maps
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
Definition: CBeacon.cpp:83
mrpt::poses::CPointPDFParticles m_locationMC
The individual PDF, if m_typePDF=pdfMonteCarlo (publicly accesible for ease of use, but the CPointPDF interface is also implemented in CBeacon).
Definition: CBeacon.h:62
static void generateRingSOG(float sensedRange, mrpt::poses::CPointPDFSOG &outPDF, const CBeaconMap *myBeaconMap, const mrpt::poses::CPoint3D &sensorPnt, const mrpt::math::CMatrixDouble33 *covarianceCompositionToAdd=nullptr, bool clearPreviousContentsOutPDF=true, const mrpt::poses::CPoint3D &centerPoint=mrpt::poses::CPoint3D(0, 0, 0), float maxDistanceFromCenter=0)
This static method returns a SOG with ring-shape (or as a 3D sphere) that can be used to initialize a...
Definition: CBeacon.cpp:457
void copyFrom(const mrpt::poses::CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Definition: CBeacon.cpp:151
void getAsMatlabDrawCommands(std::vector< std::string > &out_Str) const
Gets a set of MATLAB commands which draw the current state of the beacon:
Definition: CBeacon.cpp:287
void changeCoordinatesReference(const mrpt::poses::CPose3D &newReferenceBase) override
this = p (+) this.
Definition: CBeacon.cpp:197
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:33
mrpt::poses::CPointPDFSOG m_locationSOG
The individual PDF, if m_typePDF=pdfSOG (publicly accesible for ease of use, but the CPointPDF interf...
Definition: CBeacon.h:68
int64_t TBeaconID
The type for the IDs of landmarks.
Definition: CBeacon.h:42
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
Definition: CBeaconMap.h:43
TTypePDF m_typePDF
Which one of the different 3D point PDF is currently used in this object: montecarlo, gaussian, or a sum of gaussians.
Definition: CBeacon.h:57
A class used to store a 3D point.
Definition: CPoint3D.h:31
#define INVALID_BEACON_ID
Used for CObservationBeaconRange, CBeacon, etc.
Definition: CObservation.h:23
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s particles to a text file.
Definition: CBeacon.cpp:174
void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
Definition: CBeacon.cpp:103
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
OPENGL_SETOFOBJECTSPTR getAs3DObject() const
Returns a 3D representation of this PDF.
Definition: CPointPDF.h:94
TTypePDF
See m_typePDF.
Definition: CBeacon.h:46
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
void drawSingleSample(mrpt::poses::CPoint3D &outSample) const override
Draw a sample from the pdf.
Definition: CBeacon.cpp:128
TBeaconID m_ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
Definition: CBeacon.h:91
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:36
A probability distribution of a 2D/3D point, represented as a set of random samples (particles)...
void generateObservationModelDistribution(float sensedRange, mrpt::poses::CPointPDFSOG &outPDF, const CBeaconMap *myBeaconMap, const mrpt::poses::CPoint3D &sensorPntOnRobot, const mrpt::poses::CPoint3D &centerPoint=mrpt::poses::CPoint3D(0, 0, 0), float maxDistanceFromCenter=0) const
Compute the observation model p(z_t|x_t) for a given observation (range value), and return it as an a...
Definition: CBeacon.cpp:395
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions...
Definition: CBeacon.h:35
A gaussian distribution for 3D points.
void getMean(mrpt::poses::CPoint3D &mean_point) const override
Definition: CBeacon.cpp:63
mrpt::poses::CPointPDFGaussian m_locationGauss
The individual PDF, if m_typePDF=pdfGauss (publicly accesible for ease of use, but the CPointPDF inte...
Definition: CBeacon.h:65



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020