Main MRPT website > C++ reference
MRPT logo

CPosePDFParticles.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 CPosePDFParticles_H
00029 #define CPosePDFParticles_H
00030 
00031 #include <mrpt/poses/CPosePDF.h>
00032 #include <mrpt/poses/CPoseRandomSampler.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 
00042                 // This must be added to any CSerializable derived class:
00043                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFParticles , CPosePDF )
00044 
00045                 /** Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples.
00046                  *
00047                  *  This class is also the base for the implementation of Monte-Carlo Localization (MCL), in mrpt::slam::CMonteCarloLocalization2D.
00048                  *
00049                  *  See the application "app/pf-localization" for an example of usage.
00050                  *
00051                  * \sa CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable
00052                  */
00053                 class BASE_IMPEXP CPosePDFParticles :
00054                         public CPosePDF,
00055                         public mrpt::bayes::CParticleFilterData<CPose2D>,
00056                         public mrpt::bayes::CParticleFilterCapable
00057                 {
00058                         // This must be added to any CSerializable derived class:
00059                         DEFINE_SERIALIZABLE( CPosePDFParticles )
00060 
00061                         // This uses CParticleFilterData to implement some methods required for CParticleFilterCapable:
00062                         IMPLEMENT_PARTICLE_FILTER_CAPABLE(CPose2D);
00063 
00064                 public:
00065                         /** Free all the memory associated to m_particles, and set the number of parts = 0
00066                           */
00067                         void  clear();
00068 
00069                         /** Constructor
00070                           * \param M The number of m_particles.
00071                           */
00072                         CPosePDFParticles( size_t M = 1 );
00073 
00074                         /** Copy constructor:
00075                           */
00076                         inline CPosePDFParticles( const CPosePDFParticles& obj )
00077                         {
00078                                 copyFrom( obj );
00079                         }
00080 
00081                         /** Destructor
00082                          */
00083                         virtual ~CPosePDFParticles();
00084 
00085                         /** Copy operator, translating if necesary (for example, between m_particles and gaussian representations)
00086                           */
00087                         void  copyFrom(const CPosePDF &o);
00088 
00089                          /** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose.
00090                           * \param location The location to set all the m_particles.
00091                           * \param particlesCount If this is set to 0 the number of m_particles remains unchanged.
00092                           *  \sa resetUniform, resetUniformFreeSpace
00093                           */
00094                         void  resetDeterministic(
00095                                 const CPose2D &location,
00096                                 size_t particlesCount = 0);
00097 
00098                         /** Reset the PDF to an uniformly distributed one, inside of the defined cube.
00099                           * If particlesCount is set to -1 the number of m_particles remains unchanged.
00100                           *  \sa resetDeterministic, resetUniformFreeSpace
00101                           */
00102                         void  resetUniform(
00103                                 const double & x_min,
00104                                 const double & x_max,
00105                                 const double & y_min,
00106                                 const double & y_max,
00107                                 const double & phi_min = -M_PI,
00108                                 const double & phi_max = M_PI,
00109                                 const int       &particlesCount = -1);
00110 
00111                          /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
00112                            * \sa getCovariance
00113                            */
00114                         void getMean(CPose2D &mean_pose) const;
00115 
00116                         /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
00117                           * \sa getMean
00118                           */
00119                         void getCovarianceAndMean(CMatrixDouble33 &cov,CPose2D &mean_point) const;
00120 
00121                         /** Returns the pose of the i'th particle.
00122                           */
00123                         CPose2D  getParticlePose(size_t i) const;
00124 
00125                         /** Save PDF's m_particles to a text file. In each line it will go: "x y phi weight"
00126                          */
00127                         void  saveToTextFile(const std::string &file) const;
00128 
00129                         /** Get the m_particles count (equivalent to "particlesCount")
00130                          */
00131                         inline size_t  size() const { return m_particles.size(); }
00132 
00133                         /**  Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly.
00134                          */
00135                         void  performSubstitution( std::vector<int> &indx  );
00136 
00137                         /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
00138                           *   "to project" the current pdf. Result PDF substituted the currently stored one in the object.
00139                           */
00140                         void  changeCoordinatesReference( const CPose3D &newReferenceBase );
00141 
00142                         /** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
00143                           */
00144                         void  drawSingleSample(CPose2D &outPart ) const;
00145 
00146                         /** Appends (pose-composition) a given pose "p" to each particle
00147                           */
00148                         void  operator += ( const CPose2D &Ap);
00149 
00150                         /** Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights.
00151                           */
00152                         void  append( CPosePDFParticles &o );
00153 
00154                         /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
00155                           */
00156                         void     inverse(CPosePDF &o) const;
00157 
00158                         /** Returns the particle with the highest weight.
00159                           */
00160                         CPose2D  getMostLikelyParticle() const;
00161 
00162                         /** Bayesian fusion.
00163                           */
00164                         void  bayesianFusion( const CPosePDF &p1,const  CPosePDF &p2, const double & minMahalanobisDistToDrop = 0 );
00165 
00166                         /** Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window.
00167                           * \sa saveParzenPDFToTextFile
00168                           */
00169                         double  evaluatePDF_parzen(
00170                                 const double &  x,
00171                                 const double &  y,
00172                                 const double &  phi,
00173                                 const double &  stdXY,
00174                                 const double &  stdPhi ) const;
00175 
00176                         /** Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window.
00177                           * \sa evaluatePDF_parzen
00178                           */
00179                         void  saveParzenPDFToTextFile(
00180                                 const char              *fileName,
00181                                 const double &                  x_min,
00182                                 const double &                  x_max,
00183                                 const double &                  y_min,
00184                                 const double &                  y_max,
00185                                 const double &                  phi,
00186                                 const double &                  stepSizeXY,
00187                                 const double &                  stdXY,
00188                                 const double &                  stdPhi ) const;
00189 
00190                 }; // End of class def.
00191 
00192         } // End of namespace
00193 } // End of namespace
00194 
00195 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011