MRPT  2.0.5
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-2020, 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  ---------------------------------------------------------------*/
103  mrpt::config::CConfigFileBase& c, const std::string& s) const
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.
Declares a class for storing a collection of robot actions.
virtual double normalizeWeights(double *out_max_log_w=nullptr)=0
Normalize the (logarithmic) weights, such as the maximum weight is zero.
virtual size_t particlesCount() const =0
Get the m_particles count.
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).
virtual double getW(size_t i) const =0
Access to i&#39;th particle (logarithm) weight, where first one is index 0.
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 ...
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].
return_t square(const num_t x)
Inline function for the square of a number.
#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...
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.
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
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_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 2.0.5 Git: fc2d69e97 Fri Jul 10 18:20:57 2020 +0200 at vie jul 10 18:30:13 CEST 2020