Main MRPT website > C++ reference for MRPT 1.5.7
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 }
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
#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.
GLsizei GLsizei GLuint * obj
Definition: glext.h:3902
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...
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.
#define MRPT_END
TParticleFilterOptions()
Initilization of default parameters.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
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.
GLsizei const GLchar ** string
Definition: glext.h:3919
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0...
#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...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
TParticleFilterAlgorithm
Defines different types of particle filter algorithms.
CParticleFilter()
Default constructor.
#define ASSERT_(f)
#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.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019