53 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
55 const double w = exp((it)->log_w);
72 size_t N = m_modes.size();
74 this->getMean(estMean2D);
85 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
88 sumW +=
w = exp((it)->log_w);
91 estMean_i -= estMeanMat;
93 temp.multiply_AAt(estMean_i);
100 if (sumW != 0) estCov *= (1.0 / sumW);
116 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
120 out << (it)->
cov(0, 0) << (it)->
cov(1, 1) << (it)->
cov(2, 2);
121 out << (it)->
cov(0, 1) << (it)->
cov(0, 2) << (it)->
cov(1, 2);
145 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
150 if (version == 0) (it)->log_w = log(max(1e-300, (it)->log_w));
176 (it)->
cov(0, 0) = x0;
178 (it)->
cov(1, 1) = x0;
180 (it)->
cov(2, 2) = x0;
183 (it)->
cov(1, 0) = x0;
184 (it)->
cov(0, 1) = x0;
186 (it)->
cov(2, 0) = x0;
187 (it)->
cov(0, 2) = x0;
189 (it)->
cov(1, 2) = x0;
190 (it)->
cov(2, 1) = x0;
204 if (
this == &o)
return;
208 m_modes =
static_cast<const CPosePDFSOG*
>(&o)->m_modes;
214 m_modes[0].log_w = 0;
230 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
232 f,
"%e %e %e %e %e %e %e %e %e %e\n", exp((it)->log_w),
234 (it)->
cov(1, 1), (it)->
cov(2, 2), (it)->
cov(0, 1), (it)->
cov(0, 2),
259 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
262 (it)->
mean.composeFrom(newReferenceBase, (it)->mean);
280 rot(0, 0) = rot(1, 1) = cos(ang);
281 rot(0, 1) = -sin(ang);
282 rot(1, 0) = sin(ang);
285 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
306 size_t N, std::vector<CVectorDouble>& outSamples)
const 322 const double& minMahalanobisDistToDrop)
348 double a = -0.5 * (3 * log(
M_2PI) - log(covInv.det()) +
349 (eta.adjoint() * p2->
cov * eta)(0, 0));
351 this->m_modes.clear();
352 for (
const_iterator it = p1->m_modes.begin(); it != p1->m_modes.end(); ++it)
368 newKernel.
mean = auxGaussianProduct.
mean;
369 newKernel.
cov = auxGaussianProduct.
cov;
372 auxSOG_Kernel_i.
cov.inv(covInv_i);
375 eta_i = covInv_i * eta_i;
378 newKernel.
cov.inv(new_covInv_i);
381 new_eta_i = new_covInv_i * new_eta_i;
384 -0.5 * (3 * log(
M_2PI) - log(new_covInv_i.det()) +
385 (eta_i.adjoint() * auxSOG_Kernel_i.
cov * eta_i)(0, 0));
387 -0.5 * (3 * log(
M_2PI) - log(new_covInv_i.det()) +
388 (new_eta_i.adjoint() * newKernel.
cov * new_eta_i)(0, 0));
391 newKernel.
log_w = (it)->log_w +
a + a_i - new_a_i;
394 this->m_modes.push_back(newKernel);
413 out->
m_modes.resize(m_modes.size());
415 for (itSrc = m_modes.begin(), itDest = out->m_modes.begin();
416 itSrc != m_modes.end(); ++itSrc, ++itDest)
419 (itDest)->
mean = -(itSrc)->mean;
422 (itDest)->
cov = (itSrc)->cov;
431 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
432 (it)->mean = (it)->
mean + Ap;
434 this->rotateAllCovariances(Ap.
phi());
449 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
466 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
468 MU(0, 0) = (it)->
mean.x();
469 MU(1, 0) = (it)->
mean.y();
471 COV(0, 0) = (it)->
cov(0, 0);
472 COV(1, 1) = (it)->
cov(1, 1);
473 COV(0, 1) = COV(1, 0) = (it)->
cov(0, 1);
491 for (
const_iterator it = m_modes.begin(); it != m_modes.end(); ++it)
508 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
510 (it)->
cov(0, 1) = (it)->
cov(1, 0);
511 (it)->
cov(0, 2) = (it)->
cov(2, 0);
512 (it)->
cov(1, 2) = (it)->
cov(2, 1);
523 if (!m_modes.size())
return;
525 double maxW = m_modes[0].log_w;
526 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
527 maxW = max(maxW, (it)->log_w);
529 for (
iterator it = m_modes.begin(); it != m_modes.end(); ++it)
539 const double& x_min,
const double& x_max,
const double& y_min,
540 const double& y_max,
const double& resolutionXY,
const double& phi,
541 CMatrixD& outMatrix,
bool sumOverAllPhis)
549 const size_t Nx = (size_t)ceil((x_max - x_min) / resolutionXY);
550 const size_t Ny = (size_t)ceil((y_max - y_min) / resolutionXY);
552 outMatrix.setSize(Ny, Nx);
554 for (
size_t i = 0; i < Ny; i++)
556 double y = y_min + i * resolutionXY;
557 for (
size_t j = 0; j < Nx; j++)
559 double x = x_min + j * resolutionXY;
560 outMatrix(i, j) = evaluatePDF(
CPose2D(
x,
y, phi), sumOverAllPhis);
576 size_t N = m_modes.size();
584 for (
size_t i = 0; i < (N - 1);)
591 for (
size_t j = 0; j < N; j++) sumW += exp(m_modes[j].log_w);
594 const double Wi = exp(m_modes[i].log_w) / sumW;
596 double min_Bij = std::numeric_limits<double>::max();
604 for (
size_t j = 0; j < N; j++)
607 const double Wj = exp(m_modes[j].log_w) / sumW;
608 const double Wij_ = 1.0 / (Wi + Wj);
611 Pij.add_Ac(m_modes[j].
cov, Wj * Wij_);
619 AUX.multiply_AAt(MUij);
621 AUX *= Wi * Wj * Wij_ * Wij_;
624 double Bij = (Wi + Wj) * log(Pij.det()) -
625 Wi * log(m_modes[i].
cov.det()) -
626 Wj * log(m_modes[j].
cov.det());
629 cout <<
"try merge[" << i <<
", " << j
630 <<
"] -> Bij: " << Bij << endl;
634 cout <<
"Pij: " << Pij << endl
635 <<
" Pi: " << m_modes[i].cov << endl
636 <<
" Pj: " << m_modes[j].cov << endl;
649 cout <<
"merge[" << i <<
", " << best_j
650 <<
"] Tempting merge: KLd = " << min_Bij;
652 if (min_Bij < max_KLd)
654 if (verbose) cout <<
" Accepted." << endl;
665 const double Wj = exp(Mj.
log_w) / sumW;
666 const double Wij_ = 1.0 / (Wi + Wj);
667 const double Wi_ = Wi * Wij_;
668 const double Wj_ = Wj * Wij_;
676 Mij.
cov = min_Bij_COV;
680 m_modes.erase(m_modes.begin() + best_j);
684 if (verbose) cout <<
" Nope." << endl;
702 double best_log_w = -std::numeric_limits<double>::max();
706 if (i->log_w > best_log_w)
708 best_log_w = i->log_w;
713 if (it_best !=
end())
715 mean_point = it_best->mean;
Computes weighted and un-weighted averages of SE(2) poses.
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
double x() const
Common members of all points & poses classes.
void clear()
Clear the list of modes.
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
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...
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
CPose2D mean
The mean value.
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
The struct for each mode:
void 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"...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Declares a class that represents a Probability Density function (PDF) of a 2D pose ...
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.
double evaluatePDF(const mrpt::poses::CPose2D &x, bool sumOverAllPhis=false) const
Evaluates the PDF at a given point.
#define THROW_EXCEPTION(msg)
void getMean(CPose2D &mean_pose) const override
Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF) ...
EIGEN_STRONG_INLINE iterator begin()
void getCovariance(mrpt::math::CMatrixDouble &cov) const
Returns the estimate of the covariance matrix (STATE_LEN x STATE_LEN covariance matrix) ...
void append(const mrpt::poses::CPose2D &p)
Adds a new pose to the computation.
CMatrixFixedNumeric< double, 3, 3 > CMatrixDouble33
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
void rotateAllCovariances(const double &ang)
Rotate all the covariance matrixes by replacing them by , where .
GLubyte GLubyte GLubyte GLubyte w
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
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 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...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
CMatrixTemplateNumeric< double > CMatrixDouble
Declares a matrix of double numbers (non serializable).
mrpt::math::CMatrixDouble33 cov
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF)...
GLsizei const GLchar ** string
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...
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...
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...
void assureSymmetry()
Ensures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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.
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...
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).
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, where each row contains a (x,y,phi) datum.
const double & phi() const
Get the phi angle of the 2D pose (in radians)
void getHomogeneousMatrix(mrpt::math::CMatrixDouble44 &out_HM) const
Returns the corresponding 4x4 homogeneous transformation matrix for the point(translation) or pose (t...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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...
void readFromStream(mrpt::utils::CStream &in, int version) override
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
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...
CListGaussianModes::iterator iterator
void operator+=(const mrpt::poses::CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
unsigned __int32 uint32_t
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
double log_w
The log-weight.
GLubyte GLubyte GLubyte a
CListGaussianModes::const_iterator const_iterator
void resize(const size_t N)
Resize the number of SOG modes.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
CListGaussianModes m_modes
The list of SOG modes.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double &minMahalanobisDistToDrop=0) override
Bayesian fusion of two points gauss.
void get_average(mrpt::poses::CPose2D &out_mean) const
Returns the average pose.