MRPT  2.0.1
CPosePDFParticles.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
13 #include <mrpt/math/TPose2D.h>
14 #include <mrpt/poses/CPosePDF.h>
16 
17 namespace mrpt::poses
18 {
19 /** Declares a class that represents a Probability Density Function (PDF) over a
20  * 2D pose (x,y,phi), using a set of weighted samples.
21  *
22  * This class is also the base for the implementation of Monte-Carlo
23  * Localization (MCL), in mrpt::slam::CMonteCarloLocalization2D.
24  *
25  * See the application "app/pf-localization" for an example of usage.
26  *
27  * \sa CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable
28  * \ingroup poses_pdf_grp
29  */
31  : public CPosePDF,
33  mrpt::math::TPose2D, mrpt::bayes::particle_storage_mode::VALUE>,
35  CPosePDFParticles,
36  mrpt::bayes::CParticleFilterData<
37  mrpt::math::TPose2D,
38  mrpt::bayes::particle_storage_mode::VALUE>::CParticleList>
39 {
41 
42  public:
43  /** Free all the memory associated to m_particles, and set the number of
44  * parts = 0 */
45  void clear();
46 
47  /** Constructor
48  * \param M The number of m_particles.
49  */
50  CPosePDFParticles(size_t M = 1);
51 
52  /** Copy operator, translating if necesary (for example, between m_particles
53  * and gaussian representations)
54  */
55  void copyFrom(const CPosePDF& o) override;
56 
57  /** Reset the PDF to a single point: All m_particles will be set exactly to
58  * the supplied pose.
59  * \param location The location to set all the m_particles.
60  * \param particlesCount If this is set to 0 the number of m_particles
61  * remains unchanged.
62  * \sa resetUniform, CMonteCarloLocalization2D::resetUniformFreeSpace,
63  * resetAroundSetOfPoses
64  */
65  void resetDeterministic(
66  const mrpt::math::TPose2D& location, size_t particlesCount = 0);
67 
68  /** Reset the PDF to an uniformly distributed one, inside of the defined
69  * 2D area `[x_min,x_max]x[y_min,y_max]` (in meters) and for
70  * orientations `[phi_min, phi_max]` (in radians).
71  *
72  * \param particlesCount New particle count, or leave count unchanged if set
73  * to -1 (default).
74  *
75  * \note Orientations can be outside of the [-pi,pi] range if so desired,
76  * but it must hold `phi_max>=phi_min`.
77  * \sa resetDeterministic, CMonteCarloLocalization2D::resetUniformFreeSpace,
78  * resetAroundSetOfPoses
79  */
80  void resetUniform(
81  const double x_min, const double x_max, const double y_min,
82  const double y_max, const double phi_min = -M_PI,
83  const double phi_max = M_PI, const int particlesCount = -1);
84 
85  /** Reset the PDF to a multimodal distribution over a set of "spots"
86  * (x,y,phi)
87  * The total number of particles will be `list_poses.size() *
88  * num_particles_per_pose`.
89  * \param[in] list_poses The poses (x,y,phi) around which particles will be
90  * spread. Must contains at least one pose.
91  * \param[in] num_particles_per_pose Number of particles to be spread
92  * around each of the "spots" in list_poses. Must be >=1.
93  *
94  * Particles will be spread uniformly in a box of width
95  * `spread_{x,y,phi_rad}` in each of
96  * the three coordinates (meters, radians), so it can be understood as the
97  * "initial uncertainty".
98  *
99  * \sa resetDeterministic, CMonteCarloLocalization2D::resetUniformFreeSpace
100  */
102  const std::vector<mrpt::math::TPose2D>& list_poses,
103  const size_t num_particles_per_pose, const double spread_x,
104  const double spread_y, const double spread_phi_rad);
105 
106  void getMean(CPose2D& mean_pose) const override;
107 
108  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override;
109 
110  /** Returns the pose of the i'th particle.
111  */
112  mrpt::math::TPose2D getParticlePose(size_t i) const;
113 
114  /** Save PDF's m_particles to a text file. In each line it will go: "x y phi
115  * weight"
116  */
117  bool saveToTextFile(const std::string& file) const override;
118 
119  /** Get the m_particles count (equivalent to "particlesCount")
120  */
121  inline size_t size() const { return m_particles.size(); }
122  /** this = p (+) this. This can be used to convert a PDF from local
123  * coordinates to global, providing the point (newReferenceBase) from which
124  * "to project" the current pdf. Result PDF substituted the currently
125  * stored one in the object.
126  */
127  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
128 
129  /** Draws a single sample from the distribution (WARNING: weights are
130  * assumed to be normalized!)
131  */
132  void drawSingleSample(CPose2D& outPart) const override;
133 
134  /** Appends (pose-composition) a given pose "p" to each particle
135  */
136  void operator+=(const mrpt::math::TPose2D& Ap);
137 
138  /** Appends (add to the list) a set of m_particles to the existing ones, and
139  * then normalize weights.
140  */
141  void append(CPosePDFParticles& o);
142 
143  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
144  */
145  void inverse(CPosePDF& o) const override;
146 
147  /** Returns the particle with the highest weight.
148  */
150 
151  /** Bayesian fusion.
152  */
153  void bayesianFusion(
154  const CPosePDF& p1, const CPosePDF& p2,
155  const double minMahalanobisDistToDrop = 0) override;
156 
157  /** Evaluates the PDF at a given arbitrary point as reconstructed by a
158  * Parzen window.
159  * \sa saveParzenPDFToTextFile
160  */
161  double evaluatePDF_parzen(
162  const double x, const double y, const double phi, const double stdXY,
163  const double stdPhi) const;
164 
165  /** Save a text file (compatible with matlab) representing the 2D evaluation
166  * of the PDF as reconstructed by a Parzen window.
167  * \sa evaluatePDF_parzen
168  */
170  const char* fileName, const double x_min, const double x_max,
171  const double y_min, const double y_max, const double phi,
172  const double stepSizeXY, const double stdXY, const double stdPhi) const;
173 
174 }; // End of class def.
175 
176 } // namespace mrpt::poses
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 2D area [x_min,x_max]x[y_min,y_max] (in meters) and for orientations [phi_min, phi_max] (in radians).
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
CPosePDFParticles(size_t M=1)
Constructor.
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 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
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. ...
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:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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...
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:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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.
Definition: TPose2D.h:22
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020