MRPT  2.0.4
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 }
mrpt::bayes::CParticleFilterCapable::particlesCount
virtual size_t particlesCount() const =0
Get the m_particles count.
mrpt::bayes::CParticleFilter::TParticleFilterOptions::adaptiveSampleSize
bool adaptiveSampleSize
A flag that indicates whether the CParticleFilterCapable object should perform adative sample size (d...
Definition: CParticleFilter.h:115
bayes-precomp.h
mrpt::bayes::CParticleFilter::executeOn
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.
Definition: CParticleFilter.cpp:45
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MaximumSearchSamples
unsigned int pfAuxFilterOptimal_MaximumSearchSamples
In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal" (and in "CParticleFilter::pfAuxiliaryPFStand...
Definition: CParticleFilter.h:129
MRPT_SAVE_CONFIG_VAR_COMMENT
#define MRPT_SAVE_CONFIG_VAR_COMMENT(variableName, __comment)
Definition: config/CConfigFileBase.h:480
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:306
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
mrpt::bayes::CParticleFilter::TParticleFilterStats
Statistics for being returned from the "execute" method.
Definition: CParticleFilter.h:169
mrpt::bayes::CParticleFilter::m_options
CParticleFilter::TParticleFilterOptions m_options
The options to be used in the PF, must be set before executing any step of the particle filter.
Definition: CParticleFilter.h:209
mrpt::bayes::CParticleFilter::pfOptimalProposal
@ pfOptimalProposal
Definition: CParticleFilter.h:74
mrpt::bayes::CParticleFilter::TParticleFilterAlgorithm
TParticleFilterAlgorithm
Defines different types of particle filter algorithms.
Definition: CParticleFilter.h:70
mrpt::bayes::CParticleFilter::TParticleResamplingAlgorithm
TParticleResamplingAlgorithm
Defines the different resampling algorithms.
Definition: CParticleFilter.h:92
mrpt::bayes::CParticleFilter::TParticleFilterOptions::saveToConfigFile
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.
Definition: CParticleFilter.cpp:102
mrpt::bayes::CParticleFilter::TParticleFilterOptions::powFactor
double powFactor
An optional step to "smooth" dramatic changes in the observation model to affect the variance of the ...
Definition: CParticleFilter.h:133
mrpt::bayes::CParticleFilter::TParticleFilterStats::weightsVariance_beforeResample
double weightsVariance_beforeResample
Definition: CParticleFilter.h:172
mrpt::bayes::CParticleFilter::TParticleFilterOptions::max_loglikelihood_dyn_range
double max_loglikelihood_dyn_range
Only for PF_algorithm=pfAuxiliaryPFOptimal: If a given particle has a max_likelihood (from the a-prio...
Definition: CParticleFilter.h:148
mrpt::obs::CActionCollection
Declares a class for storing a collection of robot actions.
Definition: CActionCollection.h:26
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::utils::COutputLogger
mrpt::system::COutputLogger COutputLogger
Definition: utils/COutputLogger.h:5
CParticleFilter.h
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::bayes::CParticleFilter::TParticleFilterOptions::PF_algorithm
TParticleFilterAlgorithm PF_algorithm
The PF algorithm to use (default=pfStandardProposal) See TParticleFilterAlgorithm for the possibiliti...
Definition: CParticleFilter.h:136
MRPT_LOAD_CONFIG_VAR_NO_DEFAULT
#define MRPT_LOAD_CONFIG_VAR_NO_DEFAULT( variableName, variableType, configFileObject, sectionNameStr)
Definition: config/CConfigFileBase.h:401
mrpt::bayes::CParticleFilter::pfStandardProposal
@ pfStandardProposal
Definition: CParticleFilter.h:72
mrpt::obs::CSensoryFrame
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
CConfigFileBase.h
mrpt::bayes::CParticleFilter::TParticleFilterStats::ESS_beforeResample
double ESS_beforeResample
Definition: CParticleFilter.h:171
mrpt::bayes::CParticleFilterCapable
This virtual class defines the interface that any particles based PDF class must implement in order t...
Definition: CParticleFilterCapable.h:31
COutputLogger.h
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterStandard_FirstStageWeightsMonteCarlo
bool pfAuxFilterStandard_FirstStageWeightsMonteCarlo
Only for PF_algorithm==pfAuxiliaryPFStandard: If false, the APF will predict the first stage weights ...
Definition: CParticleFilter.h:159
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::bayes::CParticleFilter::TParticleFilterOptions::BETA
double BETA
The resampling of particles will be performed when ESS (in range [0,1]) < BETA (default is 0....
Definition: CParticleFilter.h:118
mrpt::bayes::CParticleFilterCapable::performResampling
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.
Definition: CParticleFilterCapable.cpp:29
mrpt::bayes::CParticleFilter::TParticleFilterOptions::loadFromConfigFile
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.
Definition: CParticleFilter.cpp:141
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
mrpt::bayes::CParticleFilter::CParticleFilter
CParticleFilter()
Default constructor.
Definition: CParticleFilter.cpp:40
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
CParticleFilterCapable.h
mrpt::bayes::CParticleFilterCapable::normalizeWeights
virtual double normalizeWeights(double *out_max_log_w=nullptr)=0
Normalize the (logarithmic) weights, such as the maximum weight is zero.
mrpt::bayes::CParticleFilterCapable::ESS
virtual double ESS() const =0
Returns the normalized ESS (Estimated Sample Size), in the range [0,1].
bits_math.h
mrpt::bayes::CParticleFilterCapable::getW
virtual double getW(size_t i) const =0
Access to i'th particle (logarithm) weight, where first one is index 0.
mrpt::bayes::CParticleFilterCapable::prediction_and_update
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.
Definition: CParticleFilterCapable.cpp:273
mrpt::bayes
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
Definition: CKalmanFilterCapable.h:30
mrpt::bayes::CParticleFilter::pfAuxiliaryPFOptimal
@ pfAuxiliaryPFOptimal
Definition: CParticleFilter.h:75
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::bayes::CParticleFilter::TParticleFilterOptions::sampleSize
unsigned int sampleSize
The initial number of particles in the filter (it can change only if adaptiveSampleSize=true) (defaul...
Definition: CParticleFilter.h:121
mrpt::bayes::CParticleFilter::TParticleFilterOptions::resamplingMethod
TParticleResamplingAlgorithm resamplingMethod
The resampling algorithm to use (default=prMultinomial).
Definition: CParticleFilter.h:138
mrpt::bayes::CParticleFilter::TParticleFilterOptions::pfAuxFilterOptimal_MLE
bool pfAuxFilterOptimal_MLE
(Default=false) In the algorithm "CParticleFilter::pfAuxiliaryPFOptimal", if set to true,...
Definition: CParticleFilter.h:165
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020