21 #include <Eigen/Dense> 52 size_t N = m_modes.size();
53 double X = 0, Y = 0, Z = 0;
57 CListGaussianModes::const_iterator it;
60 for (it = m_modes.begin(); it != m_modes.end(); ++it)
63 sumW +=
w = exp(it->log_w);
64 X += it->val.mean.x() *
w;
65 Y += it->val.mean.y() *
w;
66 Z += it->val.mean.z() *
w;
83 size_t N = m_modes.size();
96 CListGaussianModes::const_iterator it;
98 for (it = m_modes.begin(); it != m_modes.end(); ++it)
101 sumW +=
w = exp(it->log_w);
104 estMean_i -= estMean;
108 partCov += it->val.cov;
113 if (sumW != 0) estCov *= (1.0 / sumW);
124 for (
const auto& m : m_modes)
142 for (
auto& m : m_modes)
147 if (version == 0) m.log_w = log(max(1e-300, m.log_w));
163 if (
this == &o)
return;
167 m_modes =
static_cast<const CPointPDFSOG*
>(&o)->m_modes;
173 m_modes[0].log_w = 0;
186 if (!f)
return false;
188 for (
const auto& m_mode : m_modes)
190 f,
"%e %e %e %e %e %e %e %e %e %e\n", exp(m_mode.log_w),
191 m_mode.val.mean.x(), m_mode.val.mean.y(), m_mode.val.mean.z(),
192 m_mode.val.cov(0, 0), m_mode.val.cov(1, 1), m_mode.val.cov(2, 2),
193 m_mode.val.cov(0, 1), m_mode.val.cov(0, 2), m_mode.val.cov(1, 2));
203 for (
auto& m : m_modes) m.val.changeCoordinatesReference(newReferenceBase);
216 vector<double> logWeights(m_modes.size());
217 vector<size_t> outIdxs;
218 vector<double>::iterator itW;
219 CListGaussianModes::const_iterator it;
220 for (it = m_modes.begin(), itW = logWeights.begin(); it != m_modes.end();
224 CParticleFilterCapable::computeResampling(
225 CParticleFilter::prMultinomial,
231 size_t selectedIdx = outIdxs[0];
232 ASSERT_(selectedIdx < m_modes.size());
240 outSample.
x(selMode->mean.x() + vec[0]);
241 outSample.
y(selMode->mean.y() + vec[1]);
242 outSample.z(selMode->mean.z() + vec[2]);
252 const double minMahalanobisDistToDrop)
261 const auto* p1 =
static_cast<const CPointPDFSOG*
>(&p1_);
262 const auto* p2 =
static_cast<const CPointPDFSOG*
>(&p2_);
268 float minMahalanobisDistToDrop2 =
square(minMahalanobisDistToDrop);
270 this->m_modes.clear();
274 for (
const auto& m : p1->m_modes)
293 double a = -0.5 * (3 * log(
M_2PI) - log(covInv.
det()) +
294 (eta.transpose() *
c.asEigen() * eta)(0, 0));
296 for (
const auto& m2 : p2->m_modes)
298 auxSOG_Kernel_i = m2.val;
299 if (auxSOG_Kernel_i.
cov(2, 2) == 0)
301 auxSOG_Kernel_i.
cov(2, 2) = 1;
305 auxSOG_Kernel_i.
cov(0, 0) > 0 && auxSOG_Kernel_i.
cov(1, 1) > 0);
308 bool reallyComputeThisOne =
true;
309 if (minMahalanobisDistToDrop > 0)
314 float stdX2 = max(auxSOG_Kernel_i.
cov(0, 0), m.val.cov(0, 0));
316 square(auxSOG_Kernel_i.
mean.
x() - m.val.mean.x()) / stdX2;
318 float stdY2 = max(auxSOG_Kernel_i.
cov(1, 1), m.val.cov(1, 1));
320 square(auxSOG_Kernel_i.
mean.
y() - m.val.mean.y()) / stdY2;
325 max(auxSOG_Kernel_i.
cov(2, 2), m.val.cov(2, 2));
327 square(auxSOG_Kernel_i.
mean.z() - m.val.mean.z()) /
331 reallyComputeThisOne = mahaDist2 < minMahalanobisDistToDrop2;
334 if (reallyComputeThisOne)
349 newKernel.
val = auxGaussianProduct;
352 Eigen::Vector3d eta_i =
354 eta_i = covInv_i.
asEigen() * eta_i;
357 Eigen::Vector3d new_eta_i =
359 new_eta_i = new_covInv_i.
asEigen() * new_eta_i;
362 -0.5 * (3 * log(
M_2PI) - log(new_covInv_i.
det()) +
363 (eta_i.transpose() * auxSOG_Kernel_i.
cov.
asEigen() *
366 -0.5 * (3 * log(
M_2PI) - log(new_covInv_i.
det()) +
367 (new_eta_i.transpose() *
370 newKernel.
log_w = m.log_w + m2.log_w +
a + a_i - new_a_i;
373 if (is2D) newKernel.
val.
cov(2, 2) = 0;
376 this->m_modes.push_back(newKernel);
395 for (
auto& m_mode : m_modes)
397 m_mode.val.cov(0, 1) = m_mode.val.cov(1, 0);
398 m_mode.val.cov(0, 2) = m_mode.val.cov(2, 0);
399 m_mode.val.cov(1, 2) = m_mode.val.cov(2, 1);
412 if (!m_modes.size())
return;
414 CListGaussianModes::iterator it;
415 double maxW = m_modes[0].log_w;
416 for (it = m_modes.begin(); it != m_modes.end(); ++it)
417 maxW = max(maxW, it->log_w);
419 for (it = m_modes.begin(); it != m_modes.end(); ++it) it->log_w -= maxW;
430 CListGaussianModes::const_iterator it;
434 double sumLinearWeights = 0;
435 for (it = m_modes.begin(); it != m_modes.end(); ++it)
436 sumLinearWeights += exp(it->log_w);
439 for (it = m_modes.begin(); it != m_modes.end(); ++it)
440 cum +=
square(exp(it->log_w) / sumLinearWeights);
445 return 1.0 / (m_modes.size() * cum);
453 float x_min,
float x_max,
float y_min,
float y_max,
float resolutionXY,
454 float z,
CMatrixD& outMatrix,
bool sumOverAllZs)
462 const auto Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
463 const auto Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
466 for (
size_t i = 0; i < Ny; i++)
468 const float y = y_min + i * resolutionXY;
469 for (
size_t j = 0; j < Nx; j++)
471 float x = x_min + j * resolutionXY;
472 outMatrix(i, j) = evaluatePDF(
CPoint3D(
x,
y,
z), sumOverAllZs);
492 for (
const auto& m_mode : m_modes)
503 CMatrixD X(2, 1), MU(2, 1), COV(2, 2);
509 for (
const auto& m_mode : m_modes)
511 MU(0, 0) = m_mode.val.mean.x();
512 MU(1, 0) = m_mode.val.mean.y();
514 COV(0, 0) = m_mode.val.cov(0, 0);
515 COV(1, 1) = m_mode.val.cov(1, 1);
516 COV(0, 1) = COV(1, 0) = m_mode.val.cov(0, 1);
536 auto it_best = m_modes.end();
537 for (
auto it = m_modes.begin(); it != m_modes.end(); ++it)
538 if (it_best == m_modes.end() || it->log_w > it_best->log_w)
541 outVal = it_best->val;
A namespace of pseudo-random numbers generators of diferent distributions.
void serializeSymmetricMatrixTo(MAT &m, mrpt::serialization::CArchive &out)
Binary serialization of symmetric matrices, saving the space of duplicated values.
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
void getMean(CPoint3D &mean_point) const override
int void fclose(FILE *f)
An OS-independent version of fclose.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
CPoint3D mean
The mean value.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
void resize(const size_t N)
Resize the number of SOG modes.
Declares a class that represents a Probability Density function (PDF) of a 3D point ...
The struct for each mode:
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
void clear()
Clear all the gaussian modes.
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
GLubyte GLubyte GLubyte GLubyte w
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
T square(const T x)
Inline function for the square of a number.
#define ASSERT_(f)
Defines an assertion mechanism.
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
CMatrixFixed< double, 3, 3 > CMatrixDouble33
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
CMatrixFixed< double, 3, 1 > CMatrixDouble31
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
Scalar det() const
Determinant of matrix.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
double x() const
Common members of all points & poses classes.
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
GLsizei const GLchar ** string
A class used to store a 3D point.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Virtual base class for "archives": classes abstracting I/O streams.
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void drawGaussianMultivariate(std::vector< T > &out_result, const MATRIX &cov, const std::vector< T > *mean=nullptr)
Generate multidimensional random samples according to a given covariance matrix.
virtual std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
This file implements matrix/vector text and binary serialization.
double ESS() const
Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights o...
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
void evaluatePDFInArea(float x_min, float x_max, float y_min, float y_max, float resolutionXY, float z, mrpt::math::CMatrixD &outMatrix, bool sumOverAllZs=false)
Evaluates the PDF within a rectangular grid and saves the result in a matrix (each row contains value...
double log_w
The log-weight.
void getMostLikelyMode(CPointPDFGaussian &outVal) const
Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in t...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
unsigned __int32 uint32_t
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x...
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
GLubyte GLubyte GLubyte a
double evaluatePDF(const CPoint3D &x, bool sumOverAllZs) const
Evaluates the PDF at a given point.
bool saveToTextFile(const std::string &file) const override
Save the density to a text file, with the following format: There is one row per Gaussian "mode"...
A gaussian distribution for 3D points.
void bayesianFusion(const CPointPDF &p1, const CPointPDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)