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 CPARTICLEFILTERCAPABLE_H
00029 #define CPARTICLEFILTERCAPABLE_H
00030
00031 #include <mrpt/utils/utils_defs.h>
00032 #include <mrpt/bayes/CParticleFilter.h>
00033
00034 namespace mrpt
00035 {
00036 namespace bayes
00037 {
00043 class MRPTDLLIMPEXP CParticleFilterCapable
00044 {
00045 friend class CParticleFilter;
00046
00047 private:
00048 static const unsigned PARTICLE_FILTER_CAPABLE_FAST_DRAW_BINS;
00049
00050 public:
00051
00052 CParticleFilterCapable() : m_fastDrawAuxiliary()
00053 { }
00054
00055
00058 virtual ~CParticleFilterCapable()
00059 {
00060 }
00061
00070 typedef double ( *TParticleProbabilityEvaluator) (
00071 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00072 const CParticleFilterCapable *obj,
00073 size_t index,
00074 const void * action,
00075 const void * observation );
00076
00081 static double defaultEvaluator(
00082 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00083 const CParticleFilterCapable *obj,
00084 size_t index,
00085 const void * action,
00086 const void * observation )
00087 {
00088 MRPT_UNUSED_PARAM(action); MRPT_UNUSED_PARAM(observation);
00089 return obj->getW(index);
00090 }
00091
00110 void prepareFastDrawSample(
00111 const bayes::CParticleFilter::TParticleFilterOptions &PF_options,
00112 TParticleProbabilityEvaluator partEvaluator = defaultEvaluator,
00113 const void * action = NULL,
00114 const void * observation = NULL
00115 ) const;
00116
00127 size_t fastDrawSample( const bayes::CParticleFilter::TParticleFilterOptions &PF_options ) const;
00128
00131 virtual double getW(size_t i) const = 0;
00132
00135 virtual void setW(size_t i, double w) = 0;
00136
00139 virtual size_t particlesCount() const = 0;
00140
00145 void prediction_and_update(
00146 const mrpt::slam::CActionCollection * action,
00147 const mrpt::slam::CSensoryFrame * observation,
00148 const bayes::CParticleFilter::TParticleFilterOptions &PF_options
00149 );
00150
00154 virtual void performSubstitution( const std::vector<size_t> &indx) = 0;
00155
00160 virtual double normalizeWeights( double *out_max_log_w = NULL ) =0;
00161
00165 virtual double ESS() = 0;
00166
00173 void performResampling( const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00174
00180 static void computeResampling(
00181 CParticleFilter::TParticleResamplingAlgorithm method,
00182 const std::vector<double> &in_logWeights,
00183 std::vector<size_t> &out_indexes
00184 );
00185
00189 static void log2linearWeights(
00190 const vector_double &in_logWeights,
00191 vector_double &out_linWeights );
00192
00193
00194 protected:
00198 virtual void prediction_and_update_pfStandardProposal(
00199 const mrpt::slam::CActionCollection * action,
00200 const mrpt::slam::CSensoryFrame * observation,
00201 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00205 virtual void prediction_and_update_pfAuxiliaryPFStandard(
00206 const mrpt::slam::CActionCollection * action,
00207 const mrpt::slam::CSensoryFrame * observation,
00208 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00212 virtual void prediction_and_update_pfOptimalProposal(
00213 const mrpt::slam::CActionCollection * action,
00214 const mrpt::slam::CSensoryFrame * observation,
00215 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00219 virtual void prediction_and_update_pfAuxiliaryPFOptimal(
00220 const mrpt::slam::CActionCollection * action,
00221 const mrpt::slam::CSensoryFrame * observation,
00222 const bayes::CParticleFilter::TParticleFilterOptions &PF_options );
00223
00226 struct MRPTDLLIMPEXP TFastDrawAuxVars
00227 {
00228 TFastDrawAuxVars() :
00229 CDF(),
00230 CDF_indexes(),
00231 PDF(),
00232 alreadyDrawnIndexes(),
00233 alreadyDrawnNextOne(0)
00234 { }
00235
00236 vector_double CDF;
00237 vector_uint CDF_indexes;
00238 vector_double PDF;
00239
00240 vector_uint alreadyDrawnIndexes;
00241 size_t alreadyDrawnNextOne;
00242 };
00243
00246 mutable TFastDrawAuxVars m_fastDrawAuxiliary;
00247
00248 };
00249
00250 }
00251 }
00252 #endif