38 :
mean(init_Mean),
cov(init_Cov)
83 if (
this == &o)
return;
113 const CPose3D& newReferenceBase)
123 cov = M *
cov * M.transpose();
149 mean.
x(
x.get_unsafe(0, 0));
150 mean.
y(
x.get_unsafe(1, 0));
152 std::cout <<
"IN1: " << p1.
mean <<
"\n" << p1.
cov <<
"\n";
153 std::cout <<
"IN2: " << p2.
mean <<
"\n" << p2.
cov <<
"\n";
154 std::cout <<
"OUT: " <<
mean <<
"\n" <<
cov <<
"\n";
178 MU.get_unsafe(0, 0) =
mean.
x() -
p.mean.x();
179 MU.get_unsafe(1, 0) =
mean.
y() -
p.mean.y();
182 exp(-0.5 * MU.multiply_HtCH_scalar(C_inv));
209 outSample.
x(
mean.
x() + vec[0]);
210 outSample.
y(
mean.
y() + vec[1]);
220 const double& minMahalanobisDistToDrop)
241 Eigen::Matrix<double, 2, 1> deltaX;
247 deltaX.multiply_HtCH_scalar((other.
cov + this->cov).inverse()));
252 const double x,
const double y)
const 255 Eigen::Matrix<double, 2, 1> deltaX;
260 return std::sqrt(deltaX.multiply_HtCH_scalar(this->cov.inverse()));
A namespace of pseudo-random numbers genrators of diferent distributions.
double productIntegralNormalizedWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
CPoint2D mean
The mean value.
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.
double productIntegralWith(const CPoint2DPDFGaussian &p) const
Computes the "correspondence likelihood" of this PDF with another one: This is implemented as the int...
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const override
Introduces a pure virtual method responsible for writing to a CStream.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
A gaussian distribution for 2D points.
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.
void copyFrom(const CPoint2DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
#define THROW_EXCEPTION(msg)
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
T square(const T x)
Inline function for the square of a number.
void drawSingleSample(CPoint2D &outSample) const override
Draw a sample from the pdf.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
CPoint2DPDFGaussian()
Default constructor.
void bayesianFusion(const CPoint2DPDFGaussian &p1, const CPoint2DPDFGaussian &p2)
Bayesian fusion of two points gauss.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Declares a class that represents a Probability Distribution function (PDF) of a 2D point (x...
double mahalanobisDistanceToPoint(const double x, const double y) const
Returns the Mahalanobis distance from this PDF to some point.
GLsizei const GLchar ** string
A class used to store a 2D point.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
virtual const mrpt::utils::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
This file implements matrix/vector text and binary serialization.
void saveToTextFile(const std::string &file) const override
Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance ma...
static const size_t state_length
The length of the variable, for example, 3 for a 3D point, 6 for a 3D pose (x y z yaw pitch roll)...
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
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...
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...
CMatrixFixedNumeric< double, 2, 1 > CMatrixDouble21
void drawGaussianMultivariate(std::vector< T > &out_result, const mrpt::math::CMatrixTemplateNumeric< T > &cov, const std::vector< T > *mean=nullptr)
Generate multidimensional random samples according to a given covariance matrix.
void getRotationMatrix(mrpt::math::CMatrixDouble33 &ROT) const
Get the 3x3 rotation matrix.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
mrpt::math::CMatrixDouble22 cov
The 2x2 covariance matrix.
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...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
double mahalanobisDistanceTo(const CPoint2DPDFGaussian &other) const
Returns the Mahalanobis distance from this PDF to another PDF, that is, it's evaluation at (0...