Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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>
17 
18 namespace mrpt
19 {
20 namespace poses
21 {
22 /** Declares a class that represents a Probability Density Function (PDF) over a
23  * 2D pose (x,y,phi), using a set of weighted samples.
24  *
25  * This class is also the base for the implementation of Monte-Carlo
26  * 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,
36  mrpt::math::TPose2D, mrpt::bayes::particle_storage_mode::VALUE>,
38  CPosePDFParticles,
39  mrpt::bayes::CParticleFilterData<
40  mrpt::math::TPose2D,
41  mrpt::bayes::particle_storage_mode::VALUE>::CParticleList>
42 {
44 
45  public:
46  /** Free all the memory associated to m_particles, and set the number of
47  * parts = 0 */
48  void clear();
49 
50  /** Constructor
51  * \param M The number of m_particles.
52  */
53  CPosePDFParticles(size_t M = 1);
54 
55  /** Copy operator, translating if necesary (for example, between m_particles
56  * and gaussian representations)
57  */
58  void copyFrom(const CPosePDF& o) override;
59 
60  /** Reset the PDF to a single point: All m_particles will be set exactly to
61  * the supplied pose.
62  * \param location The location to set all the m_particles.
63  * \param particlesCount If this is set to 0 the number of m_particles
64  * remains unchanged.
65  * \sa resetUniform, resetUniformFreeSpace, resetAroundSetOfPoses
66  */
67  void resetDeterministic(
68  const mrpt::math::TPose2D& location, size_t particlesCount = 0);
69 
70  /** Reset the PDF to an uniformly distributed one, inside of the defined
71  * cube.
72  * If particlesCount is set to -1 the number of m_particles remains
73  * unchanged.
74  * \sa resetDeterministic, resetUniformFreeSpace, resetAroundSetOfPoses
75  */
76  void resetUniform(
77  const double x_min, const double x_max, const double y_min,
78  const double y_max, const double phi_min = -M_PI,
79  const double phi_max = M_PI, const int particlesCount = -1);
80 
81  /** Reset the PDF to a multimodal distribution over a set of "spots"
82  * (x,y,phi)
83  * The total number of particles will be `list_poses.size() *
84  * num_particles_per_pose`.
85  * \param[in] list_poses The poses (x,y,phi) around which particles will be
86  * spread. Must contains at least one pose.
87  * \param[in] num_particles_per_pose Number of particles to be spread
88  * around each of the "spots" in list_poses. Must be >=1.
89  *
90  * Particles will be spread uniformly in a box of width
91  * `spread_{x,y,phi_rad}` in each of
92  * the three coordinates (meters, radians), so it can be understood as the
93  * "initial uncertainty".
94  *
95  * \sa resetDeterministic, resetUniformFreeSpace
96  */
98  const std::vector<mrpt::math::TPose2D>& list_poses,
99  const size_t num_particles_per_pose, const double spread_x,
100  const double spread_y, const double spread_phi_rad);
101 
102  /** Returns an estimate of the pose, (the mean, or mathematical expectation
103  * of the PDF).
104  * \sa getCovariance
105  */
106  void getMean(CPose2D& mean_pose) const override;
107 
108  /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and
109  * the mean, both at once.
110  * \sa getMean
111  */
113  mrpt::math::CMatrixDouble33& cov, CPose2D& mean_point) const override;
114 
115  /** Returns the pose of the i'th particle.
116  */
117  mrpt::math::TPose2D getParticlePose(size_t i) const;
118 
119  /** Save PDF's m_particles to a text file. In each line it will go: "x y phi
120  * weight"
121  */
122  bool saveToTextFile(const std::string& file) const override;
123 
124  /** Get the m_particles count (equivalent to "particlesCount")
125  */
126  inline size_t size() const { return m_particles.size(); }
127  /** this = p (+) this. This can be used to convert a PDF from local
128  * coordinates to global, providing the point (newReferenceBase) from which
129  * "to project" the current pdf. Result PDF substituted the currently
130  * stored one in the object.
131  */
132  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
133 
134  /** Draws a single sample from the distribution (WARNING: weights are
135  * assumed to be normalized!)
136  */
137  void drawSingleSample(CPose2D& outPart) const override;
138 
139  /** Appends (pose-composition) a given pose "p" to each particle
140  */
141  void operator+=(const mrpt::math::TPose2D& Ap);
142 
143  /** Appends (add to the list) a set of m_particles to the existing ones, and
144  * then normalize weights.
145  */
146  void append(CPosePDFParticles& o);
147 
148  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
149  */
150  void inverse(CPosePDF& o) const override;
151 
152  /** Returns the particle with the highest weight.
153  */
155 
156  /** Bayesian fusion.
157  */
158  void bayesianFusion(
159  const CPosePDF& p1, const CPosePDF& p2,
160  const double minMahalanobisDistToDrop = 0) override;
161 
162  /** Evaluates the PDF at a given arbitrary point as reconstructed by a
163  * Parzen window.
164  * \sa saveParzenPDFToTextFile
165  */
166  double evaluatePDF_parzen(
167  const double x, const double y, const double phi,
168  const double stdXY, const double stdPhi) const;
169 
170  /** Save a text file (compatible with matlab) representing the 2D evaluation
171  * of the PDF as reconstructed by a Parzen window.
172  * \sa evaluatePDF_parzen
173  */
175  const char* fileName, const double x_min, const double x_max,
176  const double y_min, const double y_max, const double phi,
177  const double stepSizeXY, const double stdXY, const double stdPhi) const;
178 
179 }; // End of class def.
180 
181 } // End of namespace
182 } // End of namespace
183 
184 #endif
bool saveToTextFile(const std::string &file) const override
Save PDF&#39;s m_particles to a text file.
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.
CPosePDFParticles(size_t M=1)
Constructor.
GLint location
Definition: glext.h:4086
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void operator+=(const mrpt::math::TPose2D &Ap)
Appends (pose-composition) a given pose "p" to each particle.
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const override
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once...
void clear()
Free all the memory associated to m_particles, and set the number of parts = 0.
void resetDeterministic(const mrpt::math::TPose2D &location, size_t particlesCount=0)
Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose...
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) ...
void getMean(CPose2D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
mrpt::math::TPose2D getMostLikelyParticle() const
Returns the particle with the highest weight.
void saveParzenPDFToTextFile(const char *fileName, const double x_min, const double x_max, const double y_min, const double y_max, const double phi, const double stepSizeXY, const double stdXY, const double stdPhi) const
Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed ...
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
double evaluatePDF_parzen(const double x, const double y, const double phi, const double stdXY, const double stdPhi) const
Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window. ...
GLsizei const GLchar ** string
Definition: glext.h:4101
This template class declares the array of particles and its internal data, managing some memory-relat...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:41
void resetAroundSetOfPoses(const std::vector< mrpt::math::TPose2D > &list_poses, const size_t num_particles_per_pose, const double spread_x, const double spread_y, const double spread_phi_rad)
Reset the PDF to a multimodal distribution over a set of "spots" (x,y,phi) The total number of partic...
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...
size_t size() const
Get the m_particles count (equivalent to "particlesCount")
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:40
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
mrpt::math::TPose2D getParticlePose(size_t i) const
Returns the pose of the i&#39;th particle.
void append(CPosePDFParticles &o)
Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights...
Lightweight 2D pose.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion.
GLenum GLint GLint y
Definition: glext.h:3538
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:148
GLenum GLint x
Definition: glext.h:3538
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
A curiously recurring template pattern (CRTP) approach to providing the basic functionality of any CP...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019