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-2019, 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/CMatrix.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  /** Returns an estimate of the point, (the mean, or mathematical expectation
94  * of the PDF).
95  * \sa getCovariance
96  */
97  void getMean(mrpt::poses::CPoint3D& mean_point) const override;
98 
99  /** Returns an estimate of the point covariance matrix (3x3 cov matrix) and
100  * the mean, both at once.
101  * \sa getMean
102  */
105  mrpt::poses::CPoint3D& mean_point) const override;
106 
107  /** Copy operator, translating if necesary (for example, between particles
108  * and gaussian representations) */
109  void copyFrom(const mrpt::poses::CPointPDF& o) override;
110 
111  /** Save PDF's particles to a text file. See derived classes for more
112  * information about the format of generated files */
113  bool saveToTextFile(const std::string& file) const override;
114 
115  /** this = p (+) this. This can be used to convert a PDF from local
116  * coordinates to global, providing the point (newReferenceBase) from which
117  * "to project" the current pdf. Result PDF substituted the currently
118  * stored one in the object.
119  */
121  const mrpt::poses::CPose3D& newReferenceBase) override;
122 
123  /** Saves a 3D representation of the beacon into a given OpenGL scene */
125 
126  /** Gets a set of MATLAB commands which draw the current state of the
127  * beacon: */
128  void getAsMatlabDrawCommands(std::vector<std::string>& out_Str) const;
129 
130  /** Draw a sample from the pdf. */
131  void drawSingleSample(mrpt::poses::CPoint3D& outSample) const override;
132 
133  /** Bayesian fusion of two point distributions (product of two
134  * distributions->new distribution), then save the result in this object
135  * (WARNING: See implementing classes to see classes that can and cannot be
136  * mixtured!)
137  * \param p1 The first distribution to fuse
138  * \param p2 The second distribution to fuse
139  * \param minMahalanobisDistToDrop If set to different of 0, the result of
140  * very separate Gaussian modes (that will result in negligible components)
141  * in SOGs will be dropped to reduce the number of modes in the output.
142  */
143  void bayesianFusion(
144  const CPointPDF& p1, const CPointPDF& p2,
145  const double minMahalanobisDistToDrop = 0) override;
146 
147  /** Compute the observation model p(z_t|x_t) for a given observation (range
148  * value), and return it as an approximate SOG.
149  * Note that if the beacon is a SOG itself, the number of gaussian modes
150  * will be square.
151  * As a speed-up, if a "center point"+"maxDistanceFromCenter" is supplied
152  * (maxDistanceFromCenter!=0), those modes farther than this sphere will be
153  * discarded.
154  * Parameters such as the stdSigma of the sensor are gathered from
155  * "myBeaconMap"
156  * The result is one "ring" for each Gaussian mode that represent the
157  * beacon position in this object.
158  * The position of the sensor on the robot is used to shift the resulting
159  * densities such as they represent the position of the robot, not the
160  * sensor.
161  * \sa CBeaconMap::insertionOptions, generateRingSOG
162  */
164  const float& sensedRange, mrpt::poses::CPointPDFSOG& outPDF,
165  const CBeaconMap* myBeaconMap,
166  const mrpt::poses::CPoint3D& sensorPntOnRobot,
167  const mrpt::poses::CPoint3D& centerPoint =
168  mrpt::poses::CPoint3D(0, 0, 0),
169  const float& maxDistanceFromCenter = 0) const;
170 
171  /** This static method returns a SOG with ring-shape (or as a 3D sphere)
172  * that can be used to initialize a beacon if observed the first time.
173  * sensorPnt is the center of the ring/sphere, i.e. the absolute position
174  * of the range sensor.
175  * If clearPreviousContentsOutPDF=false, the SOG modes will be added to
176  * the current contents of outPDF
177  * If the 3x3 matrix covarianceCompositionToAdd is provided, it will be
178  * add to every Gaussian (to model the composition of uncertainty).
179  * \sa generateObservationModelDistribution
180  */
181  static void generateRingSOG(
182  const float& sensedRange, mrpt::poses::CPointPDFSOG& outPDF,
183  const CBeaconMap* myBeaconMap, const mrpt::poses::CPoint3D& sensorPnt,
184  const mrpt::math::CMatrixDouble33* covarianceCompositionToAdd = nullptr,
185  bool clearPreviousContentsOutPDF = true,
186  const mrpt::poses::CPoint3D& centerPoint =
187  mrpt::poses::CPoint3D(0, 0, 0),
188  const float& maxDistanceFromCenter = 0);
189 
190 }; // End of class definition
191 
192 } // namespace mrpt::maps
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
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: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
__int64 int64_t
Definition: rptypes.h:52
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
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:57
GLsizei const GLchar ** string
Definition: glext.h:4116
A class used to store a 3D point.
Definition: CPoint3D.h:30
#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:179
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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
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:82
OPENGL_SETOFOBJECTSPTR getAs3DObject() const
Returns a 3D representation of this PDF.
Definition: CPointPDF.h:94
TTypePDF
See m_typePDF.
Definition: CBeacon.h:46
void drawSingleSample(mrpt::poses::CPoint3D &outSample) const override
Draw a sample from the pdf.
Definition: CBeacon.cpp:133
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
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:463
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)...
The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions...
Definition: CBeacon.h:35
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:401
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:65



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 58f856185 Mon May 20 11:43:57 2019 +0200 at lun may 20 11:50:17 CEST 2019