9 #ifndef CPARTICLEFILTER_IMPL_H 10 #define CPARTICLEFILTER_IMPL_H 22 template <
class PARTICLEFILTERCAPABLE,
class PF_ALGORITHM>
31 obj.template prediction_and_update<PF_ALGORITHM>(
32 action, observation, m_options);
36 obj.m_poseParticles.normalizeWeights();
42 const size_t M =
obj.m_poseParticles.particlesCount();
50 double weightsMean = 0, var = 0;
51 for (
size_t i = 0; i < M; i++)
52 weightsMean += exp(
obj.m_poseParticles.getW(i));
54 for (
size_t i = 0; i < M; i++)
55 var +=
square(exp(
obj.m_poseParticles.getW(i)) - weightsMean);
64 if (!m_options.adaptiveSampleSize &&
65 (PF_ALGORITHM::DoesResampling))
67 if (
obj.m_poseParticles.ESS() < m_options.BETA)
70 "Resampling particles (ESS was %.02f)\n",
71 obj.m_poseParticles.ESS()));
72 obj.m_poseParticles.performResampling(m_options);
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
This file contains the implementations of the template members declared in mrpt::slam::PF_implementat...
Statistics for being returned from the "execute" method.
GLsizei GLsizei GLuint * obj
Declares a class for storing a collection of robot actions.
double weightsVariance_beforeResample
T square(const T x)
Inline function for the square of a number.
double ESS_beforeResample
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
#define MRPT_LOG_DEBUG(_STRING)
void executeOn(PARTICLEFILTERCAPABLE &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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.