Main MRPT website > C++ reference for MRPT 1.5.6
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-2017, 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:
22  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFParticles , CPosePDF )
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:
39  DEFINE_SERIALIZABLE( CPosePDFParticles )
40 
41  public:
42  void clear(); //!< Free all the memory associated to m_particles, and set the number of parts = 0
43 
44  /** Constructor
45  * \param M The number of m_particles.
46  */
47  CPosePDFParticles( size_t M = 1 );
48 
49  /** Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
50  */
51  void copyFrom(const CPosePDF &o) MRPT_OVERRIDE;
52 
53  /** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
54  * \param location The location to set all the m_particles.
55  * \param particlesCount If this is set to 0 the number of m_particles remains unchanged.
56  * \sa resetUniform, resetUniformFreeSpace, resetAroundSetOfPoses
57  */
58  void resetDeterministic(
59  const CPose2D &location,
60  size_t particlesCount = 0);
61 
62  /** Reset the PDF to an uniformly distributed one, inside of the defined cube.
63  * If particlesCount is set to -1 the number of m_particles remains unchanged.
64  * \sa resetDeterministic, resetUniformFreeSpace, resetAroundSetOfPoses
65  */
66  void resetUniform(
67  const double & x_min,
68  const double & x_max,
69  const double & y_min,
70  const double & y_max,
71  const double & phi_min = -M_PI,
72  const double & phi_max = M_PI,
73  const int &particlesCount = -1);
74 
75  /** Reset the PDF to a multimodal distribution over a set of "spots" (x,y,phi)
76  * The total number of particles will be `list_poses.size() * num_particles_per_pose`.
77  * \param[in] list_poses The poses (x,y,phi) around which particles will be spread. Must contains at least one pose.
78  * \param[in] num_particles_per_pose Number of particles to be spread around each of the "spots" in list_poses. Must be >=1.
79  *
80  * Particles will be spread uniformly in a box of width `spread_{x,y,phi_rad}` in each of
81  * the three coordinates (meters, radians), so it can be understood as the "initial uncertainty".
82  *
83  * \sa resetDeterministic, resetUniformFreeSpace
84  */
85  void resetAroundSetOfPoses(
86  const std::vector<mrpt::math::TPose2D> & list_poses,
87  const size_t num_particles_per_pose,
88  const double spread_x,
89  const double spread_y,
90  const double spread_phi_rad);
91 
92  /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
93  * \sa getCovariance
94  */
95  void getMean(CPose2D &mean_pose) const MRPT_OVERRIDE;
96 
97  /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
98  * \sa getMean
99  */
100  void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov,CPose2D &mean_point) const MRPT_OVERRIDE;
101 
102  /** Returns the pose of the i'th particle.
103  */
104  CPose2D getParticlePose(size_t i) const;
105 
106  /** Save PDF's m_particles to a text file. In each line it will go: "x y phi weight"
107  */
108  void saveToTextFile(const std::string &file) const MRPT_OVERRIDE;
109 
110  /** Get the m_particles count (equivalent to "particlesCount")
111  */
112  inline size_t size() const { return m_particles.size(); }
113 
114  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
115  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
116  */
117  void changeCoordinatesReference( const CPose3D &newReferenceBase ) MRPT_OVERRIDE;
118 
119  /** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
120  */
121  void drawSingleSample(CPose2D &outPart ) const MRPT_OVERRIDE;
122 
123  /** Appends (pose-composition) a given pose "p" to each particle
124  */
125  void operator += ( const CPose2D &Ap);
126 
127  /** Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
128  */
129  void append( CPosePDFParticles &o );
130 
131  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
132  */
133  void inverse(CPosePDF &o) const MRPT_OVERRIDE;
134 
135  /** Returns the particle with the highest weight.
136  */
137  CPose2D getMostLikelyParticle() const;
138 
139  /** Bayesian fusion.
140  */
141  void bayesianFusion( const CPosePDF &p1,const CPosePDF &p2, const double & minMahalanobisDistToDrop = 0 ) MRPT_OVERRIDE;
142 
143  /** Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window.
144  * \sa saveParzenPDFToTextFile
145  */
146  double evaluatePDF_parzen(
147  const double & x,
148  const double & y,
149  const double & phi,
150  const double & stdXY,
151  const double & stdPhi ) const;
152 
153  /** Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window.
154  * \sa evaluatePDF_parzen
155  */
156  void saveParzenPDFToTextFile(
157  const char *fileName,
158  const double & x_min,
159  const double & x_max,
160  const double & y_min,
161  const double & y_max,
162  const double & phi,
163  const double & stepSizeXY,
164  const double & stdXY,
165  const double & stdPhi ) const;
166 
167  }; // End of class def.
168  DEFINE_SERIALIZABLE_POST_CUSTOM_BASE( CPosePDFParticles , CPosePDF )
169 
170  } // End of namespace
171 } // End of namespace
172 
173 #endif
DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(CPose3DPDFGaussianInf, CPose3DPDF)
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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...
#define M_PI
Definition: bits.h:78
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
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
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Definition: ops_vectors.h:70
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Definition: Miscellaneous.h:74
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
Definition: eigen_frwds.h:48
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
GLsizeiptr size
Definition: glew.h:1586
#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 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
GLint location
Definition: glew.h:1742



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018