00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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/slam/CMultiMetricMap.h> 00034 #include <mrpt/bayes/CParticleFilterCapable.h> 00035 #include <mrpt/bayes/CParticleFilterData.h> 00036 #include <mrpt/slam/TKLDParams.h> 00037 00038 #include <mrpt/slam/PF_implementations_data.h> 00039 00040 namespace mrpt 00041 { 00042 namespace slam 00043 { 00044 class COccupancyGridMap2D; 00045 class CSensoryFrame; 00046 } 00047 namespace poses 00048 { 00049 using namespace mrpt::slam; 00050 using namespace mrpt::bayes; 00051 00052 // This must be added to any CSerializable derived class: 00053 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFParticles , CPosePDF ) 00054 00055 /** Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x,y,phi), using a set of weighted samples. 00056 * 00057 * This class also implements particle filtering for robot localization. See the MRPT 00058 * application "app/pf-localization" for an example of usage. 00059 * 00060 * \sa CPose2D, CPosePDF, CPoseGaussianPDF, CParticleFilterCapable 00061 */ 00062 class MRPTDLLIMPEXP CPosePDFParticles : 00063 public CPosePDF, 00064 public mrpt::slam::PF_implementation<CPose2D> 00065 { 00066 // This must be added to any CSerializable derived class: 00067 DEFINE_SERIALIZABLE( CPosePDFParticles ) 00068 00069 // This uses CParticleFilterData to implement some methods required for CParticleFilterCapable: 00070 IMPLEMENT_PARTICLE_FILTER_CAPABLE(CPose2D); 00071 00072 public: 00073 00074 /** The struct for passing extra simulation parameters to the prediction stage 00075 * when running a particle filter. 00076 * \sa prediction 00077 */ 00078 struct MRPTDLLIMPEXP TPredictionParams 00079 { 00080 /** Default settings method. 00081 */ 00082 TPredictionParams(); 00083 00084 TPredictionParams( const TPredictionParams &o ) 00085 : metricMap(o.metricMap), 00086 metricMaps(o.metricMaps), 00087 KLD_params(o.KLD_params) 00088 { 00089 } 00090 00091 TPredictionParams & operator =(const TPredictionParams &o) 00092 { 00093 metricMap = o.metricMap; 00094 metricMaps = o.metricMaps; 00095 KLD_params = o.KLD_params; 00096 return *this; 00097 } 00098 00099 /** [update stage] Must be set to a metric map used to estimate the likelihood of observations 00100 */ 00101 CMetricMap *metricMap; 00102 00103 /** [update stage] Alternative way (if metricMap==NULL): A metric map is supplied for each particle: There must be the same maps here as pose m_particles. 00104 */ 00105 TMetricMapList metricMaps; 00106 00107 TKLDParams KLD_params; //!< Parameters for dynamic sample size, KLD method. 00108 00109 } options; 00110 00111 /** Free all the memory associated to m_particles, and set the number of parts = 0 00112 */ 00113 void clear(); 00114 00115 public: 00116 00117 /** Constructor 00118 * \param M The number of m_particles. 00119 */ 00120 CPosePDFParticles( size_t M = 1 ); 00121 00122 /** Copy constructor: 00123 */ 00124 CPosePDFParticles( const CPosePDFParticles& obj ) 00125 { 00126 copyFrom( obj ); 00127 } 00128 00129 /** Destructor 00130 */ 00131 virtual ~CPosePDFParticles(); 00132 00133 00134 /** Copy operator, translating if necesary (for example, between m_particles and gaussian representations) 00135 */ 00136 void copyFrom(const CPosePDF &o); 00137 00138 /** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose. 00139 * \param location The location to set all the m_particles. 00140 * \param particlesCount If this is set to 0 the number of m_particles remains unchanged. 00141 * \sa resetUniform, resetUniformFreeSpace 00142 */ 00143 void resetDeterministic( 00144 const CPose2D &location, 00145 size_t particlesCount = 0); 00146 00147 /** Reset the PDF to an uniformly distributed one, inside of the defined cube. 00148 * If particlesCount is set to -1 the number of m_particles remains unchanged. 00149 * \sa resetDeterministic, resetUniformFreeSpace 00150 */ 00151 void resetUniform( 00152 const double & x_min, 00153 const double & x_max, 00154 const double & y_min, 00155 const double & y_max, 00156 const double & phi_min = -M_PI, 00157 const double & phi_max = M_PI, 00158 const int &particlesCount = -1); 00159 00160 /** Reset the PDF to an uniformly distributed one, but only in the free-space 00161 * of a given 2D occupancy-grid-map. Orientation is randomly generated in the whole 2*PI range. 00162 * \param theMap The occupancy grid map 00163 * \param freeCellsThreshold The minimum free-probability to consider a cell as empty (default is 0.7) 00164 * \param particlesCount If set to -1 the number of m_particles remains unchanged. 00165 * \param x_min The limits of the area to look for free cells. 00166 * \param x_max The limits of the area to look for free cells. 00167 * \param y_min The limits of the area to look for free cells. 00168 * \param y_max The limits of the area to look for free cells. 00169 * \param phi_min The limits of the area to look for free cells. 00170 * \param phi_max The limits of the area to look for free cells. 00171 * \sa resetDeterm32inistic 00172 * \exception std::exception On any error (no free cell found in map, map=NULL, etc...) 00173 */ 00174 void resetUniformFreeSpace( 00175 COccupancyGridMap2D *theMap, 00176 const double freeCellsThreshold = 0.7, 00177 const int particlesCount = -1, 00178 const double x_min = -1e10f, 00179 const double x_max = 1e10f, 00180 const double y_min = -1e10f, 00181 const double y_max = 1e10f, 00182 const double phi_min = -M_PI, 00183 const double phi_max = M_PI ); 00184 00185 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). 00186 * \sa getCovariance 00187 */ 00188 void getMean(CPose2D &mean_pose) const; 00189 00190 /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once. 00191 * \sa getMean 00192 */ 00193 void getCovarianceAndMean(CMatrixDouble33 &cov,CPose2D &mean_point) const; 00194 00195 /** Returns the pose of the i'th particle. 00196 */ 00197 CPose2D getParticlePose(size_t i) const; 00198 00199 /** Update the m_particles, predicting the posterior of robot pose and map after a movement command. 00200 * This method has additional configuration parameters in "options". 00201 * Performs the update stage of the RBPF, using the sensed Sensorial Frame: 00202 * 00203 * \param action This is a pointer to CActionCollection, containing the pose change the robot has been commanded. 00204 * \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations. 00205 * 00206 * \sa options 00207 */ 00208 void prediction_and_update_pfStandardProposal( 00209 const mrpt::slam::CActionCollection * action, 00210 const mrpt::slam::CSensoryFrame * observation, 00211 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00212 00213 /** Update the m_particles, predicting the posterior of robot pose and map after a movement command. 00214 * This method has additional configuration parameters in "options". 00215 * Performs the update stage of the RBPF, using the sensed Sensorial Frame: 00216 * 00217 * \param Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded. 00218 * \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations. 00219 * 00220 * \sa options 00221 */ 00222 void prediction_and_update_pfAuxiliaryPFStandard( 00223 const mrpt::slam::CActionCollection * action, 00224 const mrpt::slam::CSensoryFrame * observation, 00225 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00226 00227 /** Update the m_particles, predicting the posterior of robot pose and map after a movement command. 00228 * This method has additional configuration parameters in "options". 00229 * Performs the update stage of the RBPF, using the sensed Sensorial Frame: 00230 * 00231 * \param Action This is a pointer to CActionCollection, containing the pose change the robot has been commanded. 00232 * \param observation This must be a pointer to a CSensoryFrame object, with robot sensed observations. 00233 * 00234 * \sa options 00235 */ 00236 void prediction_and_update_pfAuxiliaryPFOptimal( 00237 const mrpt::slam::CActionCollection * action, 00238 const mrpt::slam::CSensoryFrame * observation, 00239 const bayes::CParticleFilter::TParticleFilterOptions &PF_options ); 00240 00241 /** Save PDF's m_particles to a text file. In each line it will go: "x y phi weight" 00242 */ 00243 void saveToTextFile(const std::string &file) const; 00244 00245 /** Get the m_particles count (equivalent to "particlesCount") 00246 */ 00247 size_t size() const { return particlesCount(); } 00248 00249 /** Performs the substitution for internal use of resample in particle filter algorithm, don't call it directly. 00250 */ 00251 void performSubstitution( std::vector<int> &indx ); 00252 00253 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00254 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00255 */ 00256 void changeCoordinatesReference( const CPose3D &newReferenceBase ); 00257 00258 /** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) 00259 */ 00260 void drawSingleSample(CPose2D &outPart ) const; 00261 00262 /** Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum. 00263 */ 00264 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00265 00266 /** Appends (pose-composition) a given pose "p" to each particle 00267 */ 00268 void operator += ( CPose2D Ap); 00269 00270 /** Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights. 00271 */ 00272 void append( CPosePDFParticles &o ); 00273 00274 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00275 */ 00276 void inverse(CPosePDF &o) const; 00277 00278 /** Returns the particle with the highest weight. 00279 */ 00280 CPose2D getMostLikelyParticle() const; 00281 00282 /** Bayesian fusion. 00283 */ 00284 void bayesianFusion( CPosePDF &p1, CPosePDF &p2, const double & minMahalanobisDistToDrop = 0 ); 00285 00286 /** Evaluates the PDF at a given arbitrary point as reconstructed by a Parzen window. 00287 * \sa saveParzenPDFToTextFile 00288 */ 00289 double evaluatePDF_parzen( 00290 const double & x, 00291 const double & y, 00292 const double & phi, 00293 const double & stdXY, 00294 const double & stdPhi ) const; 00295 00296 /** Save a text file (compatible with matlab) representing the 2D evaluation of the PDF as reconstructed by a Parzen window. 00297 * \sa evaluatePDF_parzen 00298 */ 00299 void saveParzenPDFToTextFile( 00300 const char *fileName, 00301 const double & x_min, 00302 const double & x_max, 00303 const double & y_min, 00304 const double & y_max, 00305 const double & phi, 00306 const double & stepSizeXY, 00307 const double & stdXY, 00308 const double & stdPhi ) const; 00309 00310 protected: 00311 /** \name Virtual methods that the PF_implementations assume exist. 00312 @{ */ 00313 /** Return a pointer to the last robot pose in the i'th particle (or NULL if it's a path and it's empty). */ 00314 const TPose3D * getLastPose(const size_t i) const; 00315 00316 void PF_SLAM_implementation_custom_update_particle_with_new_pose( 00317 CParticleDataContent *particleData, 00318 const TPose3D &newPose) const; 00319 00320 // We'll redefine this one: 00321 void PF_SLAM_implementation_replaceByNewParticleSet( 00322 CParticleList &old_particles, 00323 const vector<TPose3D> &newParticles, 00324 const vector_double &newParticlesWeight, 00325 const vector<size_t> &newParticlesDerivedFromIdx ) const; 00326 00327 /** Evaluate the observation likelihood for one particle at a given location */ 00328 double PF_SLAM_computeObservationLikelihoodForParticle( 00329 const CParticleFilter::TParticleFilterOptions &PF_options, 00330 const size_t particleIndexForMap, 00331 const CSensoryFrame &observation, 00332 const CPose3D &x ) const; 00333 /** @} */ 00334 00335 00336 }; // End of class def. 00337 00338 } // End of namespace 00339 } // End of namespace 00340 00341 #endif
| Page generated by Doxygen 1.7.1 for MRPT 0.8.1 SVN:exported at Sun Sep 19 14:53:45 UTC 2010 |