Main MRPT website > C++ reference
MRPT logo
CPose3DPDFParticles.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPose3DPDFParticles_H
29 #define CPose3DPDFParticles_H
30 
31 #include <mrpt/poses/CPose3DPDF.h>
32 #include <mrpt/bayes/CProbabilityParticle.h>
33 #include <mrpt/bayes/CParticleFilterCapable.h>
34 #include <mrpt/bayes/CParticleFilterData.h>
35 
36 namespace mrpt
37 {
38  namespace poses
39  {
40  using namespace mrpt::bayes;
41  using namespace mrpt::utils;
42 
43  /** \typedef CProbabilityParticle<CPose3D> CPose3DParticle
44  * A type definition for m_particles containing a 3D pose.
45  */
46  typedef CProbabilityParticle<CPose3D> CPose3DParticle;
47 
48  // This must be added to any CSerializable derived class:
49  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(CPose3DPDFParticles,CPose3DPDF)
50 
51  /** Declares a class that represents a Probability Density function (PDF) of a 3D pose
52  *
53  * This class is also the base for the implementation of Monte-Carlo Localization (MCL), in mrpt::slam::CMonteCarloLocalization2D.
54  *
55  * See the application "app/pf-localization" for an example of usage.
56  *
57  * \sa CPose3D, CPose3DPDF, CPoseGaussianPDF
58  * \ingroup poses_pdf_grp
59  */
60  class BASE_IMPEXP CPose3DPDFParticles :
61  public CPose3DPDF,
62  public mrpt::bayes::CParticleFilterData<CPose3D>,
63  public mrpt::bayes::CParticleFilterCapable
64  {
65  // This must be added to any CSerializable derived class:
66  DEFINE_SERIALIZABLE( CPose3DPDFParticles )
67 
68  // This uses CParticleFilterData to implement some methods required for CParticleFilterCapable:
69  IMPLEMENT_PARTICLE_FILTER_CAPABLE(CPose3D)
70 
71  public:
72  /** Constructor
73  * \param M The number of m_particles.
74  */
75  CPose3DPDFParticles( size_t M = 1 );
76 
77  /** Copy constructor:
78  */
79  inline CPose3DPDFParticles( const CPose3DPDFParticles& obj ) :
80  CPose3DPDF(),
81  CParticleFilterData<CPose3D>()
82  {
83  copyFrom( obj );
84  }
85 
86  /** Destructor
87  */
88  virtual ~CPose3DPDFParticles();
89 
90 
91  /** Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
92  */
93  void copyFrom(const CPose3DPDF &o);
94 
95  /** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
96  * \param location The location to set all the m_particles.
97  * \param particlesCount If this is set to 0 the number of m_particles remains unchanged.
98  * \sa resetUniform, resetUniformFreeSpace
99  */
100  void resetDeterministic( const CPose3D &location,
101  size_t particlesCount = 0);
102 
103  /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
104  * \sa getCovariance
105  */
106  void getMean(CPose3D &mean_pose) const;
107 
108  /** Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once.
109  * \sa getMean
110  */
111  void getCovarianceAndMean(CMatrixDouble66 &cov,CPose3D &mean_point) const;
112 
113  /** Returns the pose of the i'th particle.
114  */
115  CPose3D getParticlePose(int i) const;
116 
117  /** Save PDF's m_particles to a text file. In each line it will go: "x y z"
118  */
119  void saveToTextFile(const std::string &file) const;
120 
121  /** Get the m_particles count (equivalent to "particlesCount")
122  */
123  size_t size() const { return m_particles.size(); }
124 
125  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
126  * "to project" the current pdf. Result PDF substituted the currently stored one in the object.
127  */
128  void changeCoordinatesReference( const CPose3D &newReferenceBase );
129 
130  /** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
131  */
132  void drawSingleSample( CPose3D &outPart ) const;
133 
134  /** Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum.
135  */
136  void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const;
137 
138  /** Appends (pose-composition) a given pose "p" to each particle
139  */
140  void operator += ( const CPose3D &Ap);
141 
142  /** Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
143  */
144  void append( CPose3DPDFParticles &o );
145 
146  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
147  */
148  void inverse(CPose3DPDF &o) const;
149 
150  /** Returns the particle with the highest weight.
151  */
152  CPose3D getMostLikelyParticle() const;
153 
154  /** Bayesian fusion.
155  */
156  void bayesianFusion( const CPose3DPDF &p1, const CPose3DPDF &p2 );
157 
158  }; // End of class def.
159 
160 
161  } // End of namespace
162 } // End of namespace
163 
164 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013