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