Main MRPT website > C++ reference for MRPT 1.5.7
maps/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-2017, 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 
20 
21 #include <mrpt/maps/link_pragmas.h>
22 
23 
24 namespace mrpt
25 {
26  namespace utils { class CStringList; }
27 
28 namespace maps
29 {
30  class CBeaconMap;
32 
33  /** The class for storing individual "beacon landmarks" under a variety of 3D position PDF distributions.
34  * This class is used for storage within the class CBeaconMap.
35  * The class implements the same methods than the interface "CPointPDF", and invoking them actually becomes
36  * a mapping into the methods of the current PDF representation of the beacon, selectable by means of "m_typePDF"
37  * \sa CBeaconMap, CPointPDFSOG
38  * \ingroup mrpt_maps_grp
39  */
40  class MAPS_IMPEXP CBeacon : public mrpt::poses::CPointPDF
41  {
42  // This must be added to any CSerializable derived class:
44 
45  public:
46  /** The type for the IDs of landmarks.
47  */
48  typedef int64_t TBeaconID;
49 
50  /** See m_typePDF
51  */
52  enum TTypePDF { pdfMonteCarlo = 0, pdfGauss, pdfSOG };
53 
54  /** Which one of the different 3D point PDF is currently used in this 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 ease of use, but the CPointPDF interface is also implemented in CBeacon). */
61  /** The individual PDF, if m_typePDF=pdfGauss (publicly accesible for ease of use, but the CPointPDF interface is also implemented in CBeacon). */
63  /** The individual PDF, if m_typePDF=pdfSOG (publicly accesible for ease of use, but the CPointPDF interface is also implemented in CBeacon). */
65 
66  /** An ID for the landmark (see details next...)
67  * This ID was introduced in the version 3 of this class (21/NOV/2006), and its aim is
68  * to provide a way for easily establishing correspondences between landmarks detected
69  * in sequential image frames. Thus, the management of this field should be:
70  * - In 'servers' (classes/modules/... that detect landmarks from images): A different ID must be assigned to every landmark (e.g. a sequential counter), BUT only in the case of being sure of the correspondence of one landmark with another one in the past (e.g. tracking).
71  * - In 'clients': This field can be ignored, but if it is used, the advantage is solving the correspondence between landmarks detected in consequentive instants of time: Two landmarks with the same ID <b>correspond</b> to the same physical feature, BUT it should not be expected the inverse to be always true.
72  *
73  * Note that this field is never fill out automatically, it must be set by the programmer if used.
74  */
76 
77  /** Default constructor
78  */
79  CBeacon();
80 
81  /** Virtual destructor
82  */
83  virtual ~CBeacon();
84 
85  /** Returns an estimate of the point, (the mean, or mathematical expectation of the PDF).
86  * \sa getCovariance
87  */
88  void getMean(mrpt::poses::CPoint3D &mean_point) const MRPT_OVERRIDE;
89 
90  /** Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once.
91  * \sa getMean
92  */
93  void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov,mrpt::poses::CPoint3D &mean_point) const MRPT_OVERRIDE;
94 
95  /** Copy operator, translating if necesary (for example, between particles and gaussian representations) */
96  void copyFrom(const mrpt::poses::CPointPDF &o) MRPT_OVERRIDE;
97 
98  /** Save PDF's particles to a text file. See derived classes for more information about the format of generated files */
99  void saveToTextFile(const std::string &file) const MRPT_OVERRIDE;
100 
101  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
102  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
103  */
104  void changeCoordinatesReference( const mrpt::poses::CPose3D &newReferenceBase ) MRPT_OVERRIDE;
105 
106  /** Saves a 3D representation of the beacon into a given OpenGL scene */
107  void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
108 
109  /** Gets a set of MATLAB commands which draw the current state of the beacon: */
110  void getAsMatlabDrawCommands( utils::CStringList &out_Str ) const;
111 
112  /** Draw a sample from the pdf. */
113  void drawSingleSample(mrpt::poses::CPoint3D &outSample) const MRPT_OVERRIDE;
114 
115  /** 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!)
116  * \param p1 The first distribution to fuse
117  * \param p2 The second distribution to fuse
118  * \param minMahalanobisDistToDrop If set to different of 0, the result of very separate Gaussian modes (that will result in negligible components) in SOGs will be dropped to reduce the number of modes in the output.
119  */
120  void bayesianFusion(const CPointPDF &p1,const CPointPDF &p2, const double &minMahalanobisDistToDrop = 0) MRPT_OVERRIDE;
121 
122  /** Compute the observation model p(z_t|x_t) for a given observation (range value), and return it as an approximate SOG.
123  * Note that if the beacon is a SOG itself, the number of gaussian modes will be square.
124  * As a speed-up, if a "center point"+"maxDistanceFromCenter" is supplied (maxDistanceFromCenter!=0), those modes farther than this sphere will be discarded.
125  * Parameters such as the stdSigma of the sensor are gathered from "myBeaconMap"
126  * The result is one "ring" for each Gaussian mode that represent the beacon position in this object.
127  * The position of the sensor on the robot is used to shift the resulting densities such as they represent the position of the robot, not the sensor.
128  * \sa CBeaconMap::insertionOptions, generateRingSOG
129  */
130  void generateObservationModelDistribution(
131  const float &sensedRange,
132  mrpt::poses::CPointPDFSOG &outPDF,
133  const CBeaconMap *myBeaconMap,
134  const mrpt::poses::CPoint3D &sensorPntOnRobot,
135  const mrpt::poses::CPoint3D &centerPoint = mrpt::poses::CPoint3D(0,0,0),
136  const float &maxDistanceFromCenter = 0
137  ) const;
138 
139  /** This static method returns a SOG with ring-shape (or as a 3D sphere) that can be used to initialize a beacon if observed the first time.
140  * sensorPnt is the center of the ring/sphere, i.e. the absolute position of the range sensor.
141  * If clearPreviousContentsOutPDF=false, the SOG modes will be added to the current contents of outPDF
142  * If the 3x3 matrix covarianceCompositionToAdd is provided, it will be add to every Gaussian (to model the composition of uncertainty).
143  * \sa generateObservationModelDistribution
144  */
145  static void generateRingSOG(
146  const float &sensedRange,
147  mrpt::poses::CPointPDFSOG &outPDF,
148  const CBeaconMap *myBeaconMap,
149  const mrpt::poses::CPoint3D &sensorPnt,
150  const mrpt::math::CMatrixDouble33 *covarianceCompositionToAdd = NULL,
151  bool clearPreviousContentsOutPDF = true,
152  const mrpt::poses::CPoint3D &centerPoint = mrpt::poses::CPoint3D(0,0,0),
153  const float &maxDistanceFromCenter = 0
154  );
155 
156 
157  }; // End of class definition
159 
160 
161  } // End of namespace
162 } // End of namespace
163 
164 #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: maps/CBeacon.h:60
int64_t TBeaconID
The type for the IDs of landmarks.
Definition: maps/CBeacon.h:48
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:35
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
mrpt::poses::CPointPDFSOG m_locationSOG
The individual PDF, if m_typePDF=pdfSOG (publicly accesible for ease of use, but the CPointPDF interf...
Definition: maps/CBeacon.h:64
A class for storing a list of text lines.
Definition: CStringList.h:32
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before 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:135
__int64 int64_t
Definition: rptypes.h:51
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
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: maps/CBeacon.h:57
GLsizei const GLchar ** string
Definition: glext.h:3919
A class used to store a 3D point.
Definition: CPoint3D.h:32
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...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
TTypePDF
See m_typePDF.
Definition: maps/CBeacon.h:52
TBeaconID m_ID
An ID for the landmark (see details next...) This ID was introduced in the version 3 of this class (2...
Definition: maps/CBeacon.h:75
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
Definition: CPointPDF.h:38
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: maps/CBeacon.h:40
A gaussian distribution for 3D points.
mrpt::poses::CPointPDFGaussian m_locationGauss
The individual PDF, if m_typePDF=pdfGauss (publicly accesible for ease of use, but the CPointPDF inte...
Definition: maps/CBeacon.h:62



Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019