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
17namespace 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 */
54 {
55 copyFrom( obj );
56 }
57
58 /** Destructor
59 */
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 */
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.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
Definition: mrpt_macros.h:28
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.



Page generated by Doxygen 1.9.5 for MRPT 1.4.0 SVN: at Sun Nov 27 02:56:26 UTC 2022