Go to the documentation of this file.
54 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
56 const double w = exp((it)->log_w);
73 size_t N = m_modes.size();
75 this->getMean(estMean2D);
86 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
89 sumW +=
w = exp((it)->log_w);
92 estMean_i -= estMeanMat;
94 temp.multiply_AAt(estMean_i);
101 if (sumW != 0) estCov *= (1.0 / sumW);
111 for (
const auto m : m_modes)
113 out << m.log_w << m.mean;
129 for (
auto& m : m_modes)
134 if (version == 0) m.log_w = log(max(1e-300, m.log_w));
142 m.cov = mf.cast<
double>();
160 if (
this == &o)
return;
164 m_modes =
static_cast<const CPosePDFSOG*
>(&o)->m_modes;
170 m_modes[0].log_w = 0;
184 if (!f)
return false;
186 for (
const auto& m : m_modes)
188 f,
"%e %e %e %e %e %e %e %e %e %e\n", exp(m.log_w), m.mean.x(),
189 m.mean.y(), m.mean.phi(), m.cov(0, 0), m.cov(1, 1), m.cov(2, 2),
190 m.cov(0, 1), m.cov(0, 2), m.cov(1, 2));
215 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
218 (it)->
mean.composeFrom(newReferenceBase, (it)->mean);
236 rot(0, 0) = rot(1, 1) = cos(ang);
237 rot(0, 1) = -sin(ang);
238 rot(1, 0) = sin(ang);
241 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
262 size_t N, std::vector<CVectorDouble>& outSamples)
const
278 const double minMahalanobisDistToDrop)
304 double a = -0.5 * (3 * log(
M_2PI) - log(covInv.det()) +
305 (eta.adjoint() * p2->
cov * eta)(0, 0));
307 this->m_modes.clear();
324 newKernel.
mean = auxGaussianProduct.
mean;
325 newKernel.
cov = auxGaussianProduct.
cov;
328 auxSOG_Kernel_i.
cov.inv(covInv_i);
331 eta_i = covInv_i * eta_i;
334 newKernel.
cov.inv(new_covInv_i);
337 new_eta_i = new_covInv_i * new_eta_i;
340 -0.5 * (3 * log(
M_2PI) - log(new_covInv_i.det()) +
341 (eta_i.adjoint() * auxSOG_Kernel_i.
cov * eta_i)(0, 0));
343 -0.5 * (3 * log(
M_2PI) - log(new_covInv_i.det()) +
344 (new_eta_i.adjoint() * newKernel.
cov * new_eta_i)(0, 0));
347 newKernel.
log_w = (it)->log_w +
a + a_i - new_a_i;
350 this->m_modes.push_back(newKernel);
369 out->
m_modes.resize(m_modes.size());
371 for (itSrc = m_modes.begin(), itDest = out->
m_modes.begin();
372 itSrc != m_modes.end(); ++itSrc, ++itDest)
375 (itDest)->
mean = -(itSrc)->mean;
378 (itDest)->
cov = (itSrc)->cov;
387 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
388 (it)->
mean = (it)->mean + Ap;
390 this->rotateAllCovariances(Ap.
phi());
405 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
422 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
424 MU(0, 0) = (it)->
mean.x();
425 MU(1, 0) = (it)->
mean.y();
427 COV(0, 0) = (it)->
cov(0, 0);
428 COV(1, 1) = (it)->
cov(1, 1);
429 COV(0, 1) = COV(1, 0) = (it)->
cov(0, 1);
447 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
464 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
466 (it)->
cov(0, 1) = (it)->
cov(1, 0);
467 (it)->
cov(0, 2) = (it)->
cov(2, 0);
468 (it)->
cov(1, 2) = (it)->
cov(2, 1);
479 if (!m_modes.size())
return;
481 double maxW = m_modes[0].log_w;
482 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
483 maxW = max(maxW, (it)->log_w);
485 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
495 const double& x_min,
const double& x_max,
const double& y_min,
496 const double& y_max,
const double& resolutionXY,
const double& phi,
497 CMatrixD& outMatrix,
bool sumOverAllPhis)
505 const size_t Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
506 const size_t Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
508 outMatrix.setSize(Ny, Nx);
510 for (
size_t i = 0; i < Ny; i++)
512 double y = y_min + i * resolutionXY;
513 for (
size_t j = 0; j < Nx; j++)
515 double x = x_min + j * resolutionXY;
516 outMatrix(i, j) = evaluatePDF(
CPose2D(
x,
y, phi), sumOverAllPhis);
532 size_t N = m_modes.size();
540 for (
size_t i = 0; i < (N - 1);)
547 for (
size_t j = 0; j < N; j++) sumW += exp(m_modes[j].log_w);
550 const double Wi = exp(m_modes[i].log_w) / sumW;
552 double min_Bij = std::numeric_limits<double>::max();
560 for (
size_t j = 0; j < N; j++)
563 const double Wj = exp(m_modes[j].log_w) / sumW;
564 const double Wij_ = 1.0 / (Wi + Wj);
567 Pij.add_Ac(m_modes[j].
cov, Wj * Wij_);
575 AUX.multiply_AAt(MUij);
577 AUX *= Wi * Wj * Wij_ * Wij_;
580 double Bij = (Wi + Wj) * log(Pij.det()) -
581 Wi * log(m_modes[i].
cov.det()) -
582 Wj * log(m_modes[j].
cov.det());
585 cout <<
"try merge[" << i <<
", " << j
586 <<
"] -> Bij: " << Bij << endl;
590 cout <<
"Pij: " << Pij << endl
591 <<
" Pi: " << m_modes[i].cov << endl
592 <<
" Pj: " << m_modes[j].cov << endl;
605 cout <<
"merge[" << i <<
", " << best_j
606 <<
"] Tempting merge: KLd = " << min_Bij;
608 if (min_Bij < max_KLd)
610 if (verbose) cout <<
" Accepted." << endl;
621 const double Wj = exp(Mj.
log_w) / sumW;
622 const double Wij_ = 1.0 / (Wi + Wj);
623 const double Wi_ = Wi * Wij_;
624 const double Wj_ = Wj * Wij_;
632 Mij.
cov = min_Bij_COV;
636 m_modes.erase(m_modes.begin() + best_j);
640 if (verbose) cout <<
" Nope." << endl;
658 double best_log_w = -std::numeric_limits<double>::max();
662 if (i->log_w > best_log_w)
664 best_log_w = i->log_w;
669 if (it_best !=
end())
671 mean_point = it_best->mean;
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
void drawManySamples(size_t N, std::vector< mrpt::math::CVectorDouble > &outSamples) const override
Draws a number of samples from the distribution, and saves as a list of 1x3 vectors,...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void clear()
Clear the list of modes.
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two points gauss.
EIGEN_STRONG_INLINE iterator begin()
int void fclose(FILE *f)
An OS-independent version of fclose.
void getMostLikelyCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const
For the most likely Gaussian mode in the SOG, returns the pose covariance matrix (3x3 cov matrix) and...
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void getCovarianceAndMean(mrpt::math::CMatrixDouble33 &cov, CPose2D &mean_point) const override
Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
void operator+=(const mrpt::poses::CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
void serializeSymmetricMatrixTo(MAT &m, mrpt::serialization::CArchive &out)
Binary serialization of symmetric matrices, saving the space of duplicated values.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
GLubyte GLubyte GLubyte GLubyte w
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1...
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Computes weighted and un-weighted averages of SE(2) poses.
The struct for each mode:
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,...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
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 get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.
CPose2D mean
The mean value.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
double x() const
Common members of all points & poses classes.
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 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.
CListGaussianModes m_modes
The list of SOG modes.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
double evaluateNormalizedPDF(const mrpt::poses::CPose2D &x) const
Evaluates the ratio PDF(x) / max_PDF(x*), that is, the normalized PDF in the range [0,...
CListGaussianModes::iterator iterator
mrpt::math::CMatrixDouble33 cov
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
void assureSymmetry()
Ensures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
Declares a class that represents a probability density function (pdf) of a 2D pose (x,...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
void resize(const size_t N)
Resize the number of SOG modes.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
void mergeModes(double max_KLd=0.5, bool verbose=false)
Merge very close modes so the overall number of modes is reduced while preserving the total distribut...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
This base provides a set of functions for maths stuff.
double evaluatePDF(const mrpt::poses::CPose2D &x, bool sumOverAllPhis=false) const
Evaluates the PDF at a given point.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
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
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix)
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
void rotateAllCovariances(const double &ang)
Rotate all the covariance matrixes by replacing them by , where .
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
unsigned __int32 uint32_t
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
void getMean(CPose2D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF)
GLubyte GLubyte GLubyte a
Declares a class that represents a Probability Density function (PDF) of a 2D pose .
void evaluatePDFInArea(const double &x_min, const double &x_max, const double &y_min, const double &y_max, const double &resolutionXY, const double &phi, mrpt::math::CMatrixD &outMatrix, bool sumOverAllPhis=false)
Evaluates the PDF within a rectangular grid (and a fixed orientation) and saves the result in a matri...
CListGaussianModes::const_iterator const_iterator
double log_w
The log-weight.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
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 | |