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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019