MRPT  1.9.9
CParticleFilter.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "bayes-precomp.h" // Precompiled headers
11 
12 #include <mrpt/bayes/CParticleFilter.h> // for CParticleFilter::TPar...
13 #include <mrpt/bayes/CParticleFilterCapable.h> // for CParticleFilterCapable
14 #include <mrpt/config/CConfigFileBase.h> // for CConfigFileBase, MRPT...
15 #include <mrpt/core/bits_math.h> // square()
16 #include <mrpt/system/COutputLogger.h> // for COutputLogger, MRPT_L...
17 #include <cmath> // for exp
18 #include <cstddef> // for size_t
19 #include <exception> // for exception
20 #include <string> // for string, allocator
21 
22 namespace mrpt
23 {
24 namespace obs
25 {
26 class CActionCollection;
27 }
28 } // namespace mrpt
29 namespace mrpt
30 {
31 namespace obs
32 {
33 class CSensoryFrame;
34 }
35 } // namespace mrpt
36 
37 using namespace mrpt::bayes;
38 using mrpt::square;
39 
41  : mrpt::system::COutputLogger("CParticleFilter"), m_options()
42 {
43 }
44 
47  const mrpt::obs::CSensoryFrame* observation, TParticleFilterStats* stats)
48 {
50 
51  // 1,2) Prediction & Update stages:
52  // ---------------------------------------------------
53  obj.prediction_and_update(action, observation, m_options);
54 
55  // 3) Normalize weights:
56  // ---------------------------------------------------
57  obj.normalizeWeights();
58 
59  // Save weights statistics?
60  // ---------------------------------------------------
61  if (stats)
62  {
63  const size_t M = obj.particlesCount();
64 
65  // ESS:
66  stats->ESS_beforeResample = obj.ESS();
67 
68  // Variance:
69  if (M > 1)
70  {
71  double weightsMean = 0, var = 0;
72  for (size_t i = 0; i < M; i++) weightsMean += exp(obj.getW(i));
73  weightsMean /= M;
74  for (size_t i = 0; i < M; i++)
75  var += square(exp(obj.getW(i)) - weightsMean);
76 
77  var /= (M - 1);
78  stats->weightsVariance_beforeResample = var;
79  }
80  }
81 
82  // 4) Particles resampling stage
83  // ---------------------------------------------------
87  {
88  if (obj.ESS() < m_options.BETA)
89  {
91  "Resampling particles (ESS was %.02f)\n", obj.ESS()));
92  obj.performResampling(m_options); // Resample
93  }
94  }
95 
96  MRPT_END
97 }
98 
99 /*---------------------------------------------------------------
100  TParticleFilterOptions
101  ---------------------------------------------------------------*/
104 {
106  PF_algorithm, "The PF algorithm to use. See TParticleFilterAlgorithm");
109  "The resampling algorithm to use. See TParticleResamplingAlgorithm");
110 
113  "A flag that indicates whether the CParticleFilterCapable object "
114  "should perform adative sample size (default=false)");
116  BETA,
117  "The resampling of particles will be performed when ESS (in range "
118  "[0,1]) < BETA (default is 0.5)");
120  sampleSize,
121  "The initial number of particles in the filter (it can change only if "
122  "adaptiveSampleSize=true) (default=1)");
124  pfAuxFilterOptimal_MaximumSearchSamples, "See Doxygen docs");
126  powFactor,
127  "An optional step to smooth dramatic changes in the observation model "
128  "to affect the variance of the particle weights (default=1)");
131  "Only for PF_algorithm=pfAuxiliaryPFOptimal");
134  "Only for PF_algorithm==pfAuxiliaryPFStandard");
136 }
137 
138 /*---------------------------------------------------------------
139  loadFromConfigFile
140  ---------------------------------------------------------------*/
142  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
143 {
144  MRPT_START
145 
147  adaptiveSampleSize, bool, iniFile, section.c_str());
148  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(BETA, double, iniFile, section.c_str());
149  MRPT_LOAD_CONFIG_VAR_NO_DEFAULT(sampleSize, int, iniFile, section.c_str());
150  MRPT_LOAD_CONFIG_VAR(powFactor, double, iniFile, section.c_str());
152  max_loglikelihood_dyn_range, double, iniFile, section.c_str());
153  ASSERT_(max_loglikelihood_dyn_range >= 0);
154 
155  PF_algorithm = iniFile.read_enum<TParticleFilterAlgorithm>(
156  section, "PF_algorithm", PF_algorithm, true);
157  resamplingMethod = iniFile.read_enum<TParticleResamplingAlgorithm>(
158  section, "resamplingMethod", resamplingMethod, true);
159 
160  if (PF_algorithm == pfAuxiliaryPFOptimal)
161  {
163  pfAuxFilterOptimal_MaximumSearchSamples, int, iniFile,
164  section.c_str());
165  }
166  else
167  {
169  pfAuxFilterOptimal_MaximumSearchSamples, int, iniFile,
170  section.c_str());
171  }
172 
174  pfAuxFilterStandard_FirstStageWeightsMonteCarlo, bool, iniFile,
175  section.c_str());
177  pfAuxFilterOptimal_MLE, bool, iniFile, section.c_str());
178 
179  MRPT_END
180 }
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
#define MRPT_START
Definition: exceptions.h:241
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
mrpt::system::COutputLogger COutputLogger
Statistics for being returned from the "execute" method.
GLdouble s
Definition: glext.h:3682
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
Declares a class for storing a collection of robot actions.
void saveToConfigFile(mrpt::config::CConfigFileBase &target, const std::string &section) const override
This method saves the options to a ".ini"-like file or memory-stored string list. ...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
This class allows loading and storing values and vectors of different types from a configuration text...
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
TParticleResamplingAlgorithm resamplingMethod
The resampling algorithm to use (default=prMultinomial).
T square(const T x)
Inline function for the square of a number.
const GLubyte * c
Definition: glext.h:6406
string iniFile(myDataDir+string("benchmark-options.ini"))
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
This virtual class defines the interface that any particles based PDF class must implement in order t...
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo
Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...
GLsizei const GLchar ** string
Definition: glext.h:4116
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0...
#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.
void executeOn(CParticleFilterCapable &obj, const mrpt::obs::CActionCollection *action, const mrpt::obs::CSensoryFrame *observation, TParticleFilterStats *stats=nullptr)
Executes a complete prediction + update step of the selected particle filtering algorithm.
TParticleFilterAlgorithm
Defines different types of particle filter algorithms.
double max_loglikelihood_dyn_range
Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
CParticleFilter()
Default constructor.
#define MRPT_END
Definition: exceptions.h:245
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
unsigned int sampleSize
The initial number of particles in the filter (it can change only if adaptiveSampleSize=true) (defaul...
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true...
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 possibiliti...
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 479715d5b Tue Nov 12 07:26:21 2019 +0100 at mar nov 12 07:30:12 CET 2019