Main MRPT website > C++ reference for MRPT 1.5.6
CParticleFilter.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "base-precomp.h" // Precompiled headers
11 
12 #include <cmath> // for exp
13 #include <mrpt/bayes/CParticleFilter.h> // for CParticleFilter::TPar...
14 #include <mrpt/bayes/CParticleFilterCapable.h> // for CParticleFilterCapable
15 #include <mrpt/utils/CConfigFileBase.h> // for CConfigFileBase, MRPT...
16 #include <mrpt/utils/CStream.h> // for CStream
17 #include <stddef.h> // for size_t
18 #include <exception> // for exception
19 #include <string> // for string, allocator
20 #include <mrpt/utils/COutputLogger.h> // for COutputLogger, MRPT_L...
21 #include <mrpt/utils/bits.h> // for format, square
22 #include <mrpt/utils/mrpt_macros.h> // for MRPT_END, MRPT_START, e
23 namespace mrpt { namespace obs { class CActionCollection; } }
24 namespace mrpt { namespace obs { class CSensoryFrame; } }
25 
26 using namespace mrpt::bayes;
27 using namespace mrpt::utils;
28 
29 /*---------------------------------------------------------------
30  Default Constructor
31  ---------------------------------------------------------------*/
33  mrpt::utils::COutputLogger("CParticleFilter"),
34  m_options()
35 {
36 }
37 
38 /*---------------------------------------------------------------
39  executeOn
40 Executes a prediction-update stage of particle filtering. This includes:
41  - Check if ESS is below a given threshold, and if it is true resample particles.
42  - Executes a prediction with the given "action".
43  - Executes an update stage with:
44  - The PDF class update member, if updateFunctor is NULL.
45  - Using updateFunctor if one is provided.
46  - Normalization of weights.
47  ---------------------------------------------------------------*/
50  const mrpt::obs::CActionCollection *action,
51  const mrpt::obs::CSensoryFrame *observation,
52  TParticleFilterStats *stats )
53 {
55 
56  // 1,2) Prediction & Update stages:
57  // ---------------------------------------------------
58  obj.prediction_and_update( action, observation, m_options );
59 
60  // 3) Normalize weights:
61  // ---------------------------------------------------
62  obj.normalizeWeights();
63 
64  // Save weights statistics?
65  // ---------------------------------------------------
66  if (stats)
67  {
68  const size_t M = obj.particlesCount();
69 
70  // ESS:
71  stats->ESS_beforeResample = obj.ESS();
72 
73  // Variance:
74  if (M>1)
75  {
76  double weightsMean = 0, var = 0;
77  for (size_t i=0;i<M;i++) weightsMean+=exp(obj.getW(i));
78  weightsMean /= M;
79  for (size_t i=0;i<M;i++) var+=square(exp(obj.getW(i))-weightsMean);
80 
81  var/= (M-1);
82  stats->weightsVariance_beforeResample = var;
83  }
84  }
85 
86  // 4) Particles resampling stage
87  // ---------------------------------------------------
91  {
92  if (obj.ESS() < m_options.BETA)
93  {
94  MRPT_LOG_DEBUG(mrpt::format("Resampling particles (ESS was %.02f)\n", obj.ESS()));
95  obj.performResampling( m_options ); // Resample
96  }
97  }
98 
99  MRPT_END
100 }
101 
102 
103 /*---------------------------------------------------------------
104  TParticleFilterOptions
105  ---------------------------------------------------------------*/
107  adaptiveSampleSize ( false ),
108  BETA ( 0.5 ),
109  sampleSize ( 1 ),
110  pfAuxFilterOptimal_MaximumSearchSamples ( 100 ),
111  powFactor ( 1 ),
112  PF_algorithm ( pfStandardProposal ),
113  resamplingMethod ( prMultinomial ),
114  max_loglikelihood_dyn_range ( 15 ),
115  pfAuxFilterStandard_FirstStageWeightsMonteCarlo ( false ),
116  pfAuxFilterOptimal_MLE(false)
117 {
118 }
119 
120 /*---------------------------------------------------------------
121  dumpToTextStream
122  ---------------------------------------------------------------*/
124 {
125  out.printf("\n----------- [CParticleFilter::TParticleFilterOptions] ------------ \n\n");
126 
127  out.printf("PF_algorithm = ");
128  switch (PF_algorithm)
129  {
130  case CParticleFilter::pfStandardProposal: out.printf("pfStandardProposal\n"); break;
131  case CParticleFilter::pfAuxiliaryPFStandard: out.printf("pfAuxiliaryPFStandard\n"); break;
132  case CParticleFilter::pfOptimalProposal: out.printf("pfOptimalProposal\n"); break;
133  case CParticleFilter::pfAuxiliaryPFOptimal: out.printf("pfAuxiliaryPFOptimal\n"); break;
134  default:
135  out.printf("UNKNOWN!!\n"); break;
136  };
137 
138  out.printf("m_resamplingMethod = ");
139  switch (resamplingMethod)
140  {
141  case CParticleFilter::prMultinomial: out.printf("prMultinomial\n"); break;
142  case CParticleFilter::prResidual: out.printf("prResidual\n"); break;
143  case CParticleFilter::prStratified: out.printf("prStratified\n"); break;
144  case CParticleFilter::prSystematic: out.printf("prSystematic\n"); break;
145  default:
146  out.printf("UNKNOWN!!\n"); break;
147  };
148 
149  out.printf("adaptiveSampleSize = %c\n", adaptiveSampleSize ? 'Y':'N' );
150  out.printf("sampleSize = %i\n", sampleSize );
151  out.printf("BETA = %f\n", BETA );
152  out.printf("pfAuxFilterOptimal_MaximumSearchSamples = %i\n", pfAuxFilterOptimal_MaximumSearchSamples );
153  out.printf("powFactor = %f\n", powFactor);
154  out.printf("max_loglikelihood_dyn_range = %f\n", max_loglikelihood_dyn_range);
155  out.printf("pfAuxFilterStandard_FirstStageWeightsMonteCarlo = %c\n", pfAuxFilterStandard_FirstStageWeightsMonteCarlo ? 'Y':'N');
156  out.printf("pfAuxFilterOptimal_MLE = %c\n", pfAuxFilterOptimal_MLE? 'Y':'N');
157 
158  out.printf("\n");
159 }
160 
161 /*---------------------------------------------------------------
162  loadFromConfigFile
163  ---------------------------------------------------------------*/
165  const mrpt::utils::CConfigFileBase &iniFile,
166  const std::string &section)
167 {
168  MRPT_START
169 
170  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(adaptiveSampleSize,bool, iniFile,section.c_str());
171  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(BETA,double, iniFile,section.c_str());
172  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(sampleSize,int, iniFile,section.c_str());
173  MRPT_LOAD_CONFIG_VAR(powFactor,double, iniFile,section.c_str());
174  MRPT_LOAD_CONFIG_VAR(max_loglikelihood_dyn_range,double, iniFile,section.c_str());
175  ASSERT_(max_loglikelihood_dyn_range>=0)
176 
177 
178  MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(PF_algorithm,int,TParticleFilterAlgorithm, iniFile,section.c_str());
179  MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(resamplingMethod,int,TParticleResamplingAlgorithm, iniFile,section.c_str());
180 
181  if ( PF_algorithm == pfAuxiliaryPFOptimal )
182  {
183  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(pfAuxFilterOptimal_MaximumSearchSamples,int, iniFile,section.c_str());
184  }
185  else
186  {
187  MRPT_LOAD_CONFIG_VAR(pfAuxFilterOptimal_MaximumSearchSamples,int, iniFile,section.c_str());
188  }
189 
190  MRPT_LOAD_CONFIG_VAR(pfAuxFilterStandard_FirstStageWeightsMonteCarlo,bool, iniFile,section.c_str());
191  MRPT_LOAD_CONFIG_VAR(pfAuxFilterOptimal_MLE,bool, iniFile,section.c_str());
192 
193 
194  MRPT_END
195 }
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(variableName, variableType, configFileObject, sectionNameStr)
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
Statistics for being returned from the "execute" method.
Declares a class for storing a collection of robot actions.
This class allows loading and storing values and vectors of different types from a configuration text...
virtual size_t particlesCount() const =0
Get the m_particles count.
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=NULL)
Executes a complete prediction + update step of the selected particle filtering algorithm.
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
virtual double getW(size_t i) const =0
Access to i'th particle (logarithm) weight, where first one is index 0.
#define MRPT_END
TParticleFilterOptions()
Initilization of default parameters.
virtual double normalizeWeights(double *out_max_log_w=NULL)=0
Normalize the (logarithmic) weights, such as the maximum weight is zero.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
GLhandleARB obj
Definition: glew.h:3276
This virtual class defines the interface that any particles based PDF class must implement in order t...
#define MRPT_LOG_DEBUG(_STRING)
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0...
virtual double ESS() const =0
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
#define MRPT_START
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
TParticleFilterAlgorithm
Defines different types of particle filter algorithms.
GLsizei const GLcharARB ** string
Definition: glew.h:3293
void prediction_and_update(const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, const bayes::CParticleFilter::TParticleFilterOptions &PF_options)
Performs the prediction stage of the Particle Filter.
CParticleFilter()
Default constructor.
#define ASSERT_(f)
void performResampling(const bayes::CParticleFilter::TParticleFilterOptions &PF_options, size_t out_particle_count=0)
Performs a resample of the m_particles, using the method selected in the constructor.
#define MRPT_LOAD_CONFIG_VAR_CAST_NO_DEFAULT(variableName, variableType, variableTypeCast, configFileObject, sectionNameStr)
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter...
TParticleFilterAlgorithm PF_algorithm
The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the posibilitie...
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018