Main MRPT website > C++ reference
MRPT logo

CPose3DPDFParticles.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CPose3DPDFParticles_H
00029 #define CPose3DPDFParticles_H
00030 
00031 #include <mrpt/poses/CPose3DPDF.h>
00032 #include <mrpt/bayes/CProbabilityParticle.h>
00033 #include <mrpt/bayes/CParticleFilterCapable.h>
00034 #include <mrpt/bayes/CParticleFilterData.h>
00035 
00036 namespace mrpt
00037 {
00038         namespace poses
00039         {
00040                 using namespace mrpt::bayes;
00041                 using namespace mrpt::utils;
00042 
00043                 /** \typedef CProbabilityParticle<CPose3D> CPose3DParticle
00044                  *  A type definition for m_particles containing a 3D pose.
00045                  */
00046                 typedef CProbabilityParticle<CPose3D> CPose3DParticle;
00047 
00048                 // This must be added to any CSerializable derived class:
00049                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(CPose3DPDFParticles,CPose3DPDF)
00050 
00051                 /** Declares a class that represents a Probability Density function (PDF) of a 3D pose
00052                  *
00053                  *  This class is also the base for the implementation of Monte-Carlo Localization (MCL), in mrpt::slam::CMonteCarloLocalization2D.
00054                  *
00055                  *  See the application "app/pf-localization" for an example of usage.
00056                  *
00057                  * \sa CPose3D, CPose3DPDF, CPoseGaussianPDF
00058                  */
00059                 class BASE_IMPEXP CPose3DPDFParticles :
00060                         public CPose3DPDF,
00061                         public mrpt::bayes::CParticleFilterData<CPose3D>,
00062                         public mrpt::bayes::CParticleFilterCapable
00063                 {
00064                         // This must be added to any CSerializable derived class:
00065                         DEFINE_SERIALIZABLE( CPose3DPDFParticles )
00066 
00067                         // This uses CParticleFilterData to implement some methods required for CParticleFilterCapable:
00068                         IMPLEMENT_PARTICLE_FILTER_CAPABLE(CPose3D)
00069 
00070                  public:
00071                         /** Constructor
00072                           * \param M The number of m_particles.
00073                           */
00074                         CPose3DPDFParticles( size_t M = 1 );
00075 
00076                         /** Copy constructor:
00077                           */
00078                         inline CPose3DPDFParticles( const CPose3DPDFParticles& obj ) :
00079                                 CPose3DPDF(),
00080                                 CParticleFilterData<CPose3D>()
00081                         {
00082                                 copyFrom( obj );
00083                         }
00084 
00085                         /** Destructor
00086                          */
00087                         virtual ~CPose3DPDFParticles();
00088 
00089 
00090                         /** Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
00091                           */
00092                         void  copyFrom(const CPose3DPDF &o);
00093 
00094                         /** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
00095                           * \param location The location to set all the m_particles.
00096                           * \param particlesCount If this is set to 0 the number of m_particles remains unchanged.
00097                           *  \sa resetUniform, resetUniformFreeSpace
00098                           */
00099                         void  resetDeterministic( const CPose3D &location,
00100                                                                                                 size_t  particlesCount = 0);
00101 
00102                          /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles.
00103                            * \sa getCovariance
00104                            */
00105                         void getMean(CPose3D &mean_pose) const;
00106 
00107                         /** Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once.
00108                           * \sa getMean
00109                           */
00110                         void getCovarianceAndMean(CMatrixDouble66 &cov,CPose3D &mean_point) const;
00111 
00112                         /** Returns the pose of the i'th particle.
00113                           */
00114                         CPose3D  getParticlePose(int i) const;
00115 
00116                         /** Save PDF's m_particles to a text file. In each line it will go: "x y z"
00117                          */
00118                         void  saveToTextFile(const std::string &file) const;
00119 
00120                         /** Get the m_particles count (equivalent to "particlesCount")
00121                          */
00122                         size_t  size() const { return m_particles.size(); }
00123 
00124                         /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
00125                           *   "to project" the current pdf. Result PDF substituted the currently stored one in the object.
00126                           */
00127                         void  changeCoordinatesReference( const CPose3D &newReferenceBase );
00128 
00129                         /** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
00130                           */
00131                         void  drawSingleSample( CPose3D &outPart ) const;
00132 
00133                         /** 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.
00134                           */
00135                         void  drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const;
00136 
00137                         /** Appends (pose-composition) a given pose "p" to each particle
00138                           */
00139                         void  operator += ( const CPose3D &Ap);
00140 
00141                         /** Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
00142                           */
00143                         void append( CPose3DPDFParticles &o );
00144 
00145                         /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
00146                           */
00147                         void inverse(CPose3DPDF &o) const;
00148 
00149                         /** Returns the particle with the highest weight.
00150                           */
00151                         CPose3D  getMostLikelyParticle() const;
00152 
00153                         /** Bayesian fusion.
00154                           */
00155                         void  bayesianFusion( const CPose3DPDF &p1, const CPose3DPDF &p2 );
00156 
00157                 }; // End of class def.
00158 
00159 
00160         } // End of namespace
00161 } // End of namespace
00162 
00163 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011