Go to the documentation of this file.
51 size_t N = m_modes.size();
52 double X = 0, Y = 0, Z = 0;
59 for (it = m_modes.begin(); it != m_modes.end(); ++it)
62 sumW +=
w = exp(it->log_w);
63 X += it->val.mean.x() *
w;
64 Y += it->val.mean.y() *
w;
65 Z += it->val.mean.z() *
w;
86 size_t N = m_modes.size();
101 for (it = m_modes.begin(); it != m_modes.end(); ++it)
104 sumW +=
w = exp(it->log_w);
109 estMean_i -= estMean;
110 partCov.multiply_AAt(estMean_i);
111 partCov += it->val.cov;
116 if (sumW != 0) estCov *= (1.0 / sumW);
125 for (
const auto& m : m_modes)
143 for (
auto& m : m_modes)
148 if (version == 0) m.log_w = log(max(1e-300, m.log_w));
164 if (
this == &o)
return;
168 m_modes =
static_cast<const CPointPDFSOG*
>(&o)->m_modes;
174 m_modes[0].log_w = 0;
187 if (!f)
return false;
190 it != m_modes.end(); ++it)
192 f,
"%e %e %e %e %e %e %e %e %e %e\n", exp(it->log_w),
193 it->val.mean.x(), it->val.mean.y(), it->val.mean.z(),
194 it->val.cov(0, 0), it->val.cov(1, 1), it->val.cov(2, 2),
195 it->val.cov(0, 1), it->val.cov(0, 2), it->val.cov(1, 2));
205 for (
auto& m : m_modes) m.val.changeCoordinatesReference(newReferenceBase);
218 vector<double> logWeights(m_modes.size());
219 vector<size_t> outIdxs;
222 for (it = m_modes.begin(), itW = logWeights.begin(); it != m_modes.end();
226 CParticleFilterCapable::computeResampling(
227 CParticleFilter::prMultinomial,
233 size_t selectedIdx = outIdxs[0];
234 ASSERT_(selectedIdx < m_modes.size());
242 outSample.
x(selMode->
mean.
x() + vec[0]);
243 outSample.
y(selMode->
mean.
y() + vec[1]);
244 outSample.z(selMode->
mean.z() + vec[2]);
254 const double minMahalanobisDistToDrop)
270 float minMahalanobisDistToDrop2 =
square(minMahalanobisDistToDrop);
272 this->m_modes.clear();
277 it1 != p1->
m_modes.end(); ++it1)
282 if (
c.get_unsafe(2, 2) == 0)
285 c.set_unsafe(2, 2, 1);
297 double a = -0.5 * (3 * log(
M_2PI) - log(covInv.det()) +
298 eta.multiply_HtCH_scalar(
302 it2 != p2->
m_modes.end(); ++it2)
304 auxSOG_Kernel_i = (*it2).val;
305 if (auxSOG_Kernel_i.
cov.get_unsafe(2, 2) == 0)
307 auxSOG_Kernel_i.
cov.set_unsafe(2, 2, 1);
311 auxSOG_Kernel_i.
cov(0, 0) > 0 && auxSOG_Kernel_i.
cov(1, 1) > 0);
314 bool reallyComputeThisOne =
true;
315 if (minMahalanobisDistToDrop > 0)
321 max(auxSOG_Kernel_i.
cov.get_unsafe(0, 0),
322 (*it1).val.cov.get_unsafe(0, 0));
324 square(auxSOG_Kernel_i.
mean.
x() - (*it1).val.mean.x()) /
328 max(auxSOG_Kernel_i.
cov.get_unsafe(1, 1),
329 (*it1).val.cov.get_unsafe(1, 1));
331 square(auxSOG_Kernel_i.
mean.
y() - (*it1).val.mean.y()) /
337 max(auxSOG_Kernel_i.
cov.get_unsafe(2, 2),
338 (*it1).val.cov.get_unsafe(2, 2));
340 square(auxSOG_Kernel_i.
mean.z() - (*it1).val.mean.z()) /
344 reallyComputeThisOne = mahaDist2 < minMahalanobisDistToDrop2;
347 if (reallyComputeThisOne)
362 newKernel.
val = auxGaussianProduct;
366 eta_i = covInv_i * eta_i;
370 new_eta_i = new_covInv_i * new_eta_i;
374 (3 * log(
M_2PI) - log(new_covInv_i.det()) +
375 (eta_i.adjoint() * auxSOG_Kernel_i.
cov * eta_i)(0, 0));
377 -0.5 * (3 * log(
M_2PI) - log(new_covInv_i.det()) +
378 (new_eta_i.adjoint() * newKernel.
val.
cov *
382 (it1)->log_w + (it2)->log_w +
a + a_i - new_a_i;
385 if (is2D) newKernel.
val.
cov(2, 2) = 0;
388 this->m_modes.push_back(newKernel);
410 it->val.cov(0, 1) = it->val.cov(1, 0);
411 it->val.cov(0, 2) = it->val.cov(2, 0);
412 it->val.cov(1, 2) = it->val.cov(2, 1);
425 if (!m_modes.size())
return;
428 double maxW = m_modes[0].log_w;
429 for (it = m_modes.begin(); it != m_modes.end(); ++it)
430 maxW = max(maxW, it->log_w);
432 for (it = m_modes.begin(); it != m_modes.end(); ++it) it->log_w -= maxW;
447 double sumLinearWeights = 0;
448 for (it = m_modes.begin(); it != m_modes.end(); ++it)
449 sumLinearWeights += exp(it->log_w);
452 for (it = m_modes.begin(); it != m_modes.end(); ++it)
453 cum +=
square(exp(it->log_w) / sumLinearWeights);
458 return 1.0 / (m_modes.size() * cum);
466 float x_min,
float x_max,
float y_min,
float y_max,
float resolutionXY,
467 float z,
CMatrixD& outMatrix,
bool sumOverAllZs)
475 const size_t Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
476 const size_t Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
477 outMatrix.setSize(Ny, Nx);
479 for (
size_t i = 0; i < Ny; i++)
481 const float y = y_min + i * resolutionXY;
482 for (
size_t j = 0; j < Nx; j++)
484 float x = x_min + j * resolutionXY;
485 outMatrix(i, j) = evaluatePDF(
CPoint3D(
x,
y,
z), sumOverAllZs);
506 it != m_modes.end(); ++it)
517 CMatrixD X(2, 1), MU(2, 1), COV(2, 2);
524 it != m_modes.end(); ++it)
526 MU(0, 0) = it->val.mean.x();
527 MU(1, 0) = it->val.mean.y();
529 COV(0, 0) = it->val.cov(0, 0);
530 COV(1, 1) = it->val.cov(1, 1);
531 COV(0, 1) = COV(1, 0) = it->val.cov(0, 1);
552 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
553 if (it_best == m_modes.end() || it->log_w > it_best->log_w)
556 outVal = it_best->val;
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
virtual void getCovarianceAndMean(mrpt::math::CMatrixFixedNumeric< double, STATE_LEN, STATE_LEN > &cov, TDATA &mean_point) const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean,...
const Scalar * const_iterator
int void fclose(FILE *f)
An OS-independent version of fclose.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
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...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
void getMean(CPoint3D &mean_point) const override
Returns an estimate of the point, (the mean, or mathematical expectation of the PDF)
double evaluatePDF(const CPoint3D &x, bool sumOverAllZs) const
Evaluates the PDF at a given point.
void resize(const size_t N)
Resize the number of SOG modes.
void clear()
Clear all the gaussian modes.
A gaussian distribution for 3D points.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
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",...
void serializeSymmetricMatrixTo(MAT &m, mrpt::serialization::CArchive &out)
Binary serialization of symmetric matrices, saving the space of duplicated values.
GLubyte GLubyte GLubyte GLubyte w
#define ASSERT_(f)
Defines an assertion mechanism.
T square(const T x)
Inline function for the square of a number.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
double ESS() const
Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights o...
Virtual base class for "archives": classes abstracting I/O streams.
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
void drawSingleSample(CPoint3D &outSample) const override
Draw a sample from the pdf.
void bayesianFusion(const CPointPDFGaussian &p1, const CPointPDFGaussian &p2)
Bayesian fusion of two points gauss.
double log_w
The log-weight.
double x() const
Common members of all points & poses classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
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.
void getMostLikelyMode(CPointPDFGaussian &outVal) const
Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in t...
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
CListGaussianModes m_modes
The list of SOG modes.
std::deque< TGaussianMode >::const_iterator const_iterator
Declares a class that represents a Probability Density function (PDF) of a 3D point .
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
The namespace for Bayesian filtering algorithm: different particle filters and Kalman filter algorith...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void copyFrom(const CPointPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
This base provides a set of functions for maths stuff.
EIGEN_STRONG_INLINE bool empty() const
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
GLsizei const GLchar ** string
A namespace of pseudo-random numbers generators of diferent distributions.
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),...
Declares a class that represents a Probability Distribution function (PDF) of a 3D point (x,...
A class used to store a 3D point.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
The struct for each mode:
CPoint3D mean
The mean value.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
unsigned __int32 uint32_t
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPoint3D &mean_point) const override
Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
GLubyte GLubyte GLubyte a
Page generated by Doxygen 1.8.17 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at miƩ 12 jul 2023 10:03:34 CEST | |