Main MRPT website > C++ reference for MRPT 1.3.2
CPointPDFParticles.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 CPointPDFParticles_H
10 #define CPointPDFParticles_H
11 
12 #include <mrpt/poses/CPointPDF.h>
13 #include <mrpt/math/CMatrix.h>
17 
18 namespace mrpt
19 {
20 namespace poses
21 {
23 
24  /** Data within each particle
25  * \ingroup poses_pdf_grp
26  */
28  {
29  // This must be added to any CSerializable derived class:
31  public:
32  TSimple3DPoint(const TSimple3DPoint&o) : x(o.x),y(o.y),z(o.z)
33  {
34  }
35 
36  TSimple3DPoint() : x(0),y(0),z(0)
37  {
38  }
39 
40  TSimple3DPoint(const CPoint3D &v) : x(v.x()),y(v.y()),z(v.z())
41  {
42  }
43 
44  float x,y,z;
45  };
47 
49 
50  /** A probability distribution of a 2D/3D point, represented as a set of random samples (particles).
51  * \sa CPointPDF
52  * \ingroup poses_pdf_grp
53  */
54  class BASE_IMPEXP CPointPDFParticles :
55  public CPointPDF,
56  public mrpt::bayes::CParticleFilterData<TSimple3DPoint>,
57  public mrpt::bayes::CParticleFilterDataImpl<CPointPDFParticles,mrpt::bayes::CParticleFilterData<TSimple3DPoint>::CParticleList>
58  {
59  // This must be added to any CSerializable derived class:
60  DEFINE_SERIALIZABLE( CPointPDFParticles )
61 
62  public:
63  /** Default constructor
64  */
65  CPointPDFParticles(size_t numParticles = 1);
66 
67  /** Destructor
68  */
69  virtual ~CPointPDFParticles();
70 
71  /** Clear all the particles (free memory)
72  */
73  void clear() { setSize(0); }
74 
75  /** Erase all the previous particles and change the number of particles, with a given initial value
76  */
77  void setSize(size_t numberParticles, const CPoint3D &defaultValue = CPoint3D(0,0,0) );
78 
79  /** Returns the number of particles
80  */
81  size_t size() const
82  {
83  return m_particles.size();
84  }
85 
86  /** Returns an estimate of the point, (the mean, or mathematical expectation of the PDF).
87  * \sa getCovariance
88  */
89  void getMean(CPoint3D &mean_point) const;
90 
91  /** Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once.
92  * \sa getMean
93  */
94  void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov,CPoint3D &mean_point) const;
95 
96  /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
97  */
98  void copyFrom(const CPointPDF &o);
99 
100  /** Save PDF's particles to a text file, where each line is: X Y Z LOG_W
101  */
102  void saveToTextFile(const std::string &file) const;
103 
104  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
105  * "to project" the current pdf. Result PDF substituted the currently stored one in the object. Both the mean value and the covariance matrix are updated correctly.
106  */
107  void changeCoordinatesReference( const CPose3D &newReferenceBase );
108 
109  /** Compute the kurtosis of the distribution.
110  */
111  double computeKurtosis();
112 
113  /** Draw a sample from the pdf.
114  */
115  void drawSingleSample(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  }; // End of class def.
126 
127 
128  } // End of namespace
129 } // End of namespace
130 
131 #endif
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...
TSimple3DPoint(const CPoint3D &v)
A numeric matrix of compile-time fixed size.
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
class BASE_IMPEXP CSerializable
Definition: CStream.h:23
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
Data within each particle.
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...
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
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)...
size_t size() const
Returns the number of particles.



Page generated by Doxygen 1.8.11 for MRPT 1.3.2 SVN: at Wed May 25 02:34:21 UTC 2016