00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
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
00051 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFParticles , CPosePDF )
00052
00053
00060 class MRPTDLLIMPEXP CPosePDFParticles : public CPosePDF, public mrpt::bayes::CParticleFilterCapable, public mrpt::bayes::CParticleFilterData<CPose2D>
00061 {
00062
00063 DEFINE_SERIALIZABLE( CPosePDFParticles )
00064
00065
00066 IMPLEMENT_PARTICLE_FILTER_CAPABLE(CPose2D);
00067
00068 public:
00069
00074 struct MRPTDLLIMPEXP TPredictionParams
00075 {
00078 TPredictionParams();
00079
00080 TPredictionParams( const TPredictionParams &o )
00081 : metricMap(o.metricMap),
00082 metricMaps(o.metricMaps),
00083 KLD_params(o.KLD_params)
00084 {
00085 }
00086
00087 TPredictionParams & operator =(const TPredictionParams &o)
00088 {
00089 metricMap = o.metricMap;
00090 metricMaps = o.metricMaps;
00091 KLD_params = o.KLD_params;
00092 return *this;
00093 }
00094
00097 CMetricMap *metricMap;
00098
00101 TMetricMapList metricMaps;
00102
00103 TKLDParams KLD_params;
00104
00105 } options;
00106
00109 void clear();
00110
00111 public:
00112
00116 CPosePDFParticles( size_t M = 1 );
00117
00120 CPosePDFParticles( const CPosePDFParticles& obj ) :
00121 CPosePDF(),
00122 bayes::CParticleFilterCapable(),
00123 CParticleFilterData<CPose2D>(),
00124 options(),
00125 m_pfAuxiliaryPFOptimal_estimatedProb(0)
00126 {
00127 copyFrom( obj );
00128 }
00129
00132 virtual ~CPosePDFParticles();
00133
00134
00137 void copyFrom(const CPosePDF &o);
00138
00144 void resetDeterministic(
00145 const CPose2D &location,
00146 size_t particlesCount = 0);
00147
00152 void resetUniform(
00153 const double & x_min,
00154 const double & x_max,
00155 const double & y_min,
00156 const double & y_max,
00157 const double & phi_min = -M_PI,
00158 const double & phi_max = M_PI,
00159 const int &particlesCount = -1);
00160
00175 void resetUniformFreeSpace(
00176 COccupancyGridMap2D *theMap,
00177 const double & freeCellsThreshold = 0.7,
00178 const int & particlesCount = -1,
00179 const double & x_min = -1e10f,
00180 const double & x_max = 1e10f,
00181 const double & y_min = -1e10f,
00182 const double & y_max = 1e10f,
00183 const double & phi_min = -M_PI,
00184 const double & phi_max = M_PI );
00185
00189 CPose2D getEstimatedPose() const;
00190
00193 CMatrixD getEstimatedCovariance() const;
00194
00197 CPose2D getParticlePose(size_t i) const;
00198
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
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
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
00243 void saveToTextFile(const std::string &file) const;
00244
00247 size_t size() const { return particlesCount(); }
00248
00251 void performSubstitution( std::vector<int> &indx );
00252
00256 void changeCoordinatesReference( const CPose2D &newReferenceBase );
00257
00260 void drawSingleSample(CPose2D &outPart ) const;
00261
00264 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const;
00265
00268 void operator += ( CPose2D Ap);
00269
00272 void append( CPosePDFParticles &o );
00273
00276 void inverse(CPosePDF &o) const;
00277
00280 CPose2D getMostLikelyParticle() const;
00281
00284 void bayesianFusion( CPosePDF &p1, CPosePDF &p2, const double & minMahalanobisDistToDrop = 0 );
00285
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
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 private:
00313 struct MRPTDLLIMPEXP TPoseBin
00314 {
00315 int x,y,phi;
00316 };
00319 struct MRPTDLLIMPEXP lt_TPoseBin
00320 {
00321 bool operator()(const TPoseBin& s1, const TPoseBin& s2) const
00322 {
00323 return s1.x < s2.x;
00324 }
00325 };
00326
00329 mutable vector_double m_pfAuxiliaryPFOptimal_estimatedProb;
00330
00333 mutable vector_double m_pfAuxiliaryPFStandard_estimatedProb;
00334
00337 CPoseRandomSampler m_movementDrawer;
00338
00341 static double particlesEvaluator_AuxPFStandard(
00342 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00343 const CParticleFilterCapable *obj,
00344 size_t index,
00345 const void * action,
00346 const void * observation );
00347
00348
00351 static double particlesEvaluator_AuxPFOptimal(
00352 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00353 const CParticleFilterCapable *obj,
00354 size_t index,
00355 const void * action,
00356 const void * observation );
00357
00360 static double auxiliarComputeObservationLikelihood(
00361 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00362 const CParticleFilterCapable *obj,
00363 size_t particleIndexForMap,
00364 const CSensoryFrame *observation,
00365 const CPose2D *x );
00366
00367 };
00368
00369
00370 }
00371 }
00372
00373 #endif