37 m_particles.resize(M);
39 for (
auto&
p : m_particles)
46 resetDeterministic(nullPose);
49 void CPosePDFParticles::copyFrom(
const CPosePDF& o)
56 if (
this == &o)
return;
70 size_t M = m_particles.size();
71 std::vector<CVectorDouble> parts;
77 m_particles.resize(M);
79 for (itDest = m_particles.begin(), partsIt = parts.begin();
80 itDest != m_particles.end(); ++itDest, ++partsIt)
85 (pdf->
mean.
x() + (*partsIt)[0]),
86 (pdf->
mean.
y() + (*partsIt)[1]),
87 (pdf->
mean.
phi() + (*partsIt)[2])));
88 itDest->d->normalizePhi();
102 void CPosePDFParticles::getMean(
CPose2D& est_)
const 105 const size_t n = m_particles.size();
109 for (
size_t i = 0; i <
n; i++)
111 const CPose2D&
p = *m_particles[i].d;
112 double w = exp(m_particles[i].log_w);
126 void CPosePDFParticles::getCovarianceAndMean(
132 size_t i,
n = m_particles.size();
133 double var_x = 0, var_y = 0, var_p = 0, var_xy = 0, var_xp = 0, var_yp = 0;
134 double mean_phi =
mean.phi();
136 if (mean_phi < 0) mean_phi =
M_2PI + mean_phi;
138 double lin_w_sum = 0;
140 for (i = 0; i <
n; i++) lin_w_sum += exp(m_particles[i].log_w);
141 if (lin_w_sum == 0) lin_w_sum = 1;
143 for (i = 0; i <
n; i++)
145 double w = exp(m_particles[i].log_w) / lin_w_sum;
148 double err_x = m_particles[i].d->x() -
mean.x();
149 double err_y = m_particles[i].d->y() -
mean.y();
156 var_xy += err_x * err_y *
w;
157 var_xp += err_x * err_phi *
w;
158 var_yp += err_y * err_phi *
w;
172 cov(1, 0) =
cov(0, 1) = var_xy;
173 cov(2, 0) =
cov(0, 2) = var_xp;
174 cov(1, 2) =
cov(2, 1) = var_yp;
181 void CPosePDFParticles::writeToStream(
188 writeParticlesToStream(out);
201 readParticlesFromStream(
in);
213 void CPosePDFParticles::resetDeterministic(
216 if (particlesCount > 0)
219 m_particles.resize(particlesCount);
220 for (
auto&
p : m_particles)
p.d.resetDefaultCtor();
223 for (
auto&
p : m_particles)
233 void CPosePDFParticles::resetUniform(
234 const double& x_min,
const double& x_max,
const double& y_min,
235 const double& y_max,
const double& phi_min,
const double& phi_max,
236 const int& particlesCount)
240 if (particlesCount > 0)
243 m_particles.resize(particlesCount);
244 for (
int i = 0; i < particlesCount; i++)
245 m_particles[i].d.reset(
new CPose2D());
248 size_t i, M = m_particles.size();
249 for (i = 0; i < M; i++)
254 m_particles[i].log_w = 0;
260 void CPosePDFParticles::resetAroundSetOfPoses(
261 const std::vector<mrpt::math::TPose2D>& list_poses,
262 const size_t num_particles_per_pose,
const double spread_x,
263 const double spread_y,
const double spread_phi_rad)
267 ASSERT_(num_particles_per_pose >= 1);
269 const size_t N = list_poses.size() * num_particles_per_pose;
272 m_particles.resize(N);
274 for (i = 0, nSpot = 0; nSpot < list_poses.size(); nSpot++)
277 for (
size_t k = 0; k < num_particles_per_pose; k++, i++)
279 m_particles[i].d.reset(
new CPose2D());
282 p.x - spread_x * 0.5,
p.x + spread_x * 0.5));
285 p.y - spread_y * 0.5,
p.y + spread_y * 0.5));
286 m_particles[i].d->phi(
288 p.phi - spread_phi_rad * 0.5,
289 p.phi + spread_phi_rad * 0.5));
290 m_particles[i].log_w = 0;
310 for (
unsigned int i = 0; i < m_particles.size(); i++)
312 f,
"%f %f %f %e\n", m_particles[i].d->x(), m_particles[i].d->y(),
313 m_particles[i].d->phi(), m_particles[i].log_w);
321 CPose2D CPosePDFParticles::getParticlePose(
size_t i)
const 323 return *m_particles[i].d;
329 void CPosePDFParticles::changeCoordinatesReference(
330 const CPose3D& newReferenceBase_)
335 it != m_particles.end(); ++it)
336 it->d->composeFrom(newReferenceBase, *it->d);
342 void CPosePDFParticles::drawSingleSample(
CPose2D& outPart)
const 348 it != m_particles.end(); ++it)
350 cum += exp(it->log_w);
359 outPart = *(m_particles.rbegin())->d;
368 it != m_particles.end(); ++it)
369 it->d->composeFrom(*it->d, Ap);
377 for (
unsigned int i = 0; i < o.
m_particles.size(); i++)
382 m_particles.push_back(part);
398 static CPose2D nullPose(0, 0, 0);
400 for (
unsigned int i = 0; i < out->m_particles.size(); i++)
401 (*out->m_particles[i].d) = nullPose - (*out->m_particles[i].d);
409 CPose2D CPosePDFParticles::getMostLikelyParticle()
const 412 double max_w = -1e300;
414 for (it = m_particles.begin(); it != m_particles.end(); ++it)
416 if (it->log_w > max_w)
429 void CPosePDFParticles::bayesianFusion(
431 const double& minMahalanobisDistToDrop)
443 double CPosePDFParticles::evaluatePDF_parzen(
444 const double&
x,
const double&
y,
const double& phi,
const double& stdXY,
445 const double& stdPhi)
const 450 it != m_particles.end(); ++it)
454 ret += exp(it->log_w) *
465 void CPosePDFParticles::saveParzenPDFToTextFile(
466 const char* fileName,
const double& x_min,
const double& x_max,
467 const double& y_min,
const double& y_max,
const double& phi,
468 const double& stepSizeXY,
const double& stdXY,
const double& stdPhi)
const 473 for (
double y = y_min;
y < y_max;
y += stepSizeXY)
475 for (
double x = x_min;
x < x_max;
x += stepSizeXY)
477 os::fprintf(f,
"%f ", evaluatePDF_parzen(
x,
y, phi, stdXY, stdPhi));
#define ASSERT_EQUAL_(__A, __B)
Computes weighted and un-weighted averages of SE(2) poses.
A namespace of pseudo-random numbers genrators of diferent distributions.
double drawUniform(const double Min, const double Max)
Generate a uniformly distributed pseudo-random number using the MT19937 algorithm, scaled to the selected range.
double x() const
Common members of all points & poses classes.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
CPose2D mean
The mean value.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
int void fclose(FILE *f)
An OS-independent version of fclose.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
#define THROW_EXCEPTION(msg)
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Save matrix to a text file, compatible with MATLAB text format (see also the methods of matrix classe...
const Scalar * const_iterator
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void clear()
Clear the contents of this container.
GLubyte GLubyte GLubyte GLubyte w
T square(const T x)
Inline function for the square of a number.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
CParticleList m_particles
The array of particles.
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between m_particles and gaussian representations...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=nullptr)
Generate a given number of multidimensional random samples according to a given covariance matrix...
std::vector< T1 > & operator+=(std::vector< T1 > &a, const std::vector< T2 > &b)
a+=b (element-wise sum)
Eigen::Matrix< dataType, 4, 4 > inverse(Eigen::Matrix< dataType, 4, 4 > &pose)
Declares a class that represents a Probability Density Function (PDF) over a 2D pose (x...
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A template class for holding a the data and the weight of a particle.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
const double & phi() const
Get the phi angle of the 2D pose (in radians)
double log_w
The (logarithmic) weight value for this particle.
mrpt::utils::copy_ptr< T > d
The data associated with this particle.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.