Main MRPT website > C++ reference for MRPT 1.3.2
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-2015, 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;
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;
94 
95  /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
96  */
97  void copyFrom(const mrpt::poses::CPointPDF &o);
98 
99  /** Save PDF's particles to a text file. See derived classes for more information about the format of generated files.
100  */
101  void saveToTextFile(const std::string &file) const;
102 
103  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
104  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
105  */
106  void changeCoordinatesReference( const mrpt::poses::CPose3D &newReferenceBase );
107 
108  /** Saves a 3D representation of the beacon into a given OpenGL scene */
109  void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
110 
111  /** Gets a set of MATLAB commands which draw the current state of the beacon: */
112  void getAsMatlabDrawCommands( utils::CStringList &out_Str ) const;
113 
114  /** Draw a sample from the pdf. */
115  void drawSingleSample(mrpt::poses::CPoint3D &outSample) const;
116 
117  /** 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!)
118  * \param p1 The first distribution to fuse
119  * \param p2 The second distribution to fuse
120  * \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.
121  */
122  void bayesianFusion(const CPointPDF &p1,const CPointPDF &p2, const double &minMahalanobisDistToDrop = 0);
123 
124  /** Compute the observation model p(z_t|x_t) for a given observation (range value), and return it as an approximate SOG.
125  * Note that if the beacon is a SOG itself, the number of gaussian modes will be square.
126  * As a speed-up, if a "center point"+"maxDistanceFromCenter" is supplied (maxDistanceFromCenter!=0), those modes farther than this sphere will be discarded.
127  * Parameters such as the stdSigma of the sensor are gathered from "myBeaconMap"
128  * The result is one "ring" for each Gaussian mode that represent the beacon position in this object.
129  * 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.
130  * \sa CBeaconMap::insertionOptions, generateRingSOG
131  */
132  void generateObservationModelDistribution(
133  const float &sensedRange,
135  const CBeaconMap *myBeaconMap,
136  const mrpt::poses::CPoint3D &sensorPntOnRobot,
137  const mrpt::poses::CPoint3D &centerPoint = mrpt::poses::CPoint3D(0,0,0),
138  const float &maxDistanceFromCenter = 0
139  ) const;
140 
141  /** 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.
142  * sensorPnt is the center of the ring/sphere, i.e. the absolute position of the range sensor.
143  * If clearPreviousContentsOutPDF=false, the SOG modes will be added to the current contents of outPDF
144  * If the 3x3 matrix covarianceCompositionToAdd is provided, it will be add to every Gaussian (to model the composition of uncertainty).
145  * \sa generateObservationModelDistribution
146  */
147  static void generateRingSOG(
148  const float &sensedRange,
150  const CBeaconMap *myBeaconMap,
151  const mrpt::poses::CPoint3D &sensorPnt,
152  const mrpt::math::CMatrixDouble33 *covarianceCompositionToAdd = NULL,
153  bool clearPreviousContentsOutPDF = true,
154  const mrpt::poses::CPoint3D &centerPoint = mrpt::poses::CPoint3D(0,0,0),
155  const float &maxDistanceFromCenter = 0
156  );
157 
158 
159  }; // End of class definition
161 
162 
163  } // End of namespace
164 } // End of namespace
165 
166 #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
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...
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
Definition: CPointPDFSOG.h:35
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
A numeric matrix of compile-time fixed size.
#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
A class for storing a map of 3D probabilistic beacons, using a Montecarlo, Gaussian, or Sum of Gaussians (SOG) representation (for range-only SLAM).
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
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.11 for MRPT 1.3.2 SVN: at Mon May 9 06:50:38 UTC 2016