Main MRPT website > C++ reference for MRPT 1.4.0
CPosePDFParticles.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-2016, 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 CPosePDFParticles_H
10 #define CPosePDFParticles_H
11 
12 #include <mrpt/poses/CPosePDF.h>
16 
17 namespace mrpt
18 {
19  namespace poses
20  {
21  // This must be added to any CSerializable derived class:
23 
24  /** Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.
25  *
26  * This class is also the base for the implementation of Monte-Carlo Localization (MCL), in mrpt::slam::CMonteCarloLocalization2D.
27  *
28  * See the application "app/pf-localization" for an example of usage.
29  *
30  * \sa CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable
31  * \ingroup poses_pdf_grp
32  */
34  public CPosePDF,
35  public mrpt::bayes::CParticleFilterData<CPose2D>,
36  public mrpt::bayes::CParticleFilterDataImpl<CPosePDFParticles,mrpt::bayes::CParticleFilterData<CPose2D>::CParticleList>
37  {
38  // This must be added to any CSerializable derived class:
40 
41  public:
42  /** Free all the memory associated to m_particles, and set the number of parts = 0
43  */
44  void clear();
45 
46  /** Constructor
47  * \param M The number of m_particles.
48  */
49  CPosePDFParticles( size_t M = 1 );
50 
51  /** Copy constructor:
52  */
53  inline CPosePDFParticles( const CPosePDFParticles& obj )
54  {
55  copyFrom( obj );
56  }
57 
58  /** Destructor
59  */
60  virtual ~CPosePDFParticles();
61 
62  /** Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
63  */
65 
66  /** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
67  * \param location The location to set all the m_particles.
68  * \param particlesCount If this is set to 0 the number of m_particles remains unchanged.
69  * \sa resetUniform, resetUniformFreeSpace
70  */
72  const CPose2D &location,
73  size_t particlesCount = 0);
74 
75  /** Reset the PDF to an uniformly distributed one, inside of the defined cube.
76  * If particlesCount is set to -1 the number of m_particles remains unchanged.
77  * \sa resetDeterministic, resetUniformFreeSpace
78  */
80  const double & x_min,
81  const double & x_max,
82  const double & y_min,
83  const double & y_max,
84  const double & phi_min = -M_PI,
85  const double & phi_max = M_PI,
86  const int &particlesCount = -1);
87 
88  /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
89  * \sa getCovariance
90  */
91  void getMean(CPose2D &mean_pose) const MRPT_OVERRIDE;
92 
93  /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
94  * \sa getMean
95  */
97 
98  /** Returns the pose of the i'th particle.
99  */
100  CPose2D getParticlePose(size_t i) const;
101 
102  /** Save PDF's m_particles to a text file. In each line it will go: "x y phi weight"
103  */
104  void saveToTextFile(const std::string &file) const MRPT_OVERRIDE;
105 
106  /** Get the m_particles count (equivalent to "particlesCount")
107  */
108  inline size_t size() const { return m_particles.size(); }
109 
110  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
111  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
112  */
113  void changeCoordinatesReference( const CPose3D &newReferenceBase ) MRPT_OVERRIDE;
114 
115  /** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
116  */
117  void drawSingleSample(CPose2D &outPart ) const MRPT_OVERRIDE;
118 
119  /** Appends (pose-composition) a given pose "p" to each particle
120  */
121  void operator += ( const CPose2D &Ap);
122 
123  /** Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
124  */
126 
127  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
128  */
130 
131  /** Returns the particle with the highest weight.
132  */
134 
135  /** Bayesian fusion.
136  */
137  void bayesianFusion( const CPosePDF &p1,const CPosePDF &p2, const double & minMahalanobisDistToDrop = 0 ) MRPT_OVERRIDE;
138 
139  /** Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window.
140  * \sa saveParzenPDFToTextFile
141  */
142  double evaluatePDF_parzen(
143  const double & x,
144  const double & y,
145  const double & phi,
146  const double & stdXY,
147  const double & stdPhi ) const;
148 
149  /** Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window.
150  * \sa evaluatePDF_parzen
151  */
152  void saveParzenPDFToTextFile(
153  const char *fileName,
154  const double & x_min,
155  const double & x_max,
156  const double & y_min,
157  const double & y_max,
158  const double & phi,
159  const double & stepSizeXY,
160  const double & stdXY,
161  const double & stdPhi ) const;
162 
163  }; // End of class def.
165 
166  } // End of namespace
167 } // End of namespace
168 
169 #endif
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(class_name, base_name)
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define M_PI
Definition: bits.h:65
std::deque< CParticleData > CParticleList
Use this type to refer to the list of particles m_particles.
A numeric matrix of compile-time fixed size.
A class used to store a 2D pose.
Definition: CPose2D.h:37
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:73
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
Definition: CPosePDF.h:40
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,...
void append(CPosePDFParticles &o)
Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
void drawSingleSample(CPose2D &outPart) const MRPT_OVERRIDE
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!...
CPosePDFParticles(size_t M=1)
Constructor.
void copyFrom(const CPosePDF &o) MRPT_OVERRIDE
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
void inverse(CPosePDF &o) const MRPT_OVERRIDE
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void resetDeterministic(const CPose2D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
CPose2D getMostLikelyParticle() const
Returns the particle with the highest weight.
void saveToTextFile(const std::string &file) const MRPT_OVERRIDE
Save PDF's m_particles to a text file.
CPosePDFParticles(const CPosePDFParticles &obj)
Copy constructor:
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) MRPT_OVERRIDE
Bayesian fusion.
CPose2D getParticlePose(size_t i) const
Returns the pose of the i'th particle.
virtual ~CPosePDFParticles()
Destructor.
void clear()
Free all the memory associated to m_particles, and set the number of parts = 0.
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const MRPT_OVERRIDE
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
void changeCoordinatesReference(const CPose3D &newReferenceBase) MRPT_OVERRIDE
this = p (+) this.
void getMean(CPose2D &mean_pose) const MRPT_OVERRIDE
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
void resetUniform(const double &x_min, const double &x_max, const double &y_min, const double &y_max, const double &phi_min=-M_PI, const double &phi_max=M_PI, const int &particlesCount=-1)
Reset the PDF to an uniformly distributed one, inside of the defined cube.
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
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
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.1 for MRPT 1.4.0 SVN: at Mon Apr 18 03:37:47 UTC 2022