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