Go to the documentation of this file.
42 :
mean(init_Mean), cov_inv(init_CovInv)
50 :
mean(init_Mean), cov_inv()
69 in >>
p.x >>
p.y >>
p.phi;
86 if (
this == &o)
return;
131 o_cov(0, 0) = C(0, 0);
132 o_cov(1, 1) = C(1, 1);
133 o_cov(2, 2) = C(3, 3);
134 o_cov(0, 1) = o_cov(1, 0) = C(0, 1);
135 o_cov(0, 2) = o_cov(2, 0) = C(0, 3);
136 o_cov(1, 2) = o_cov(2, 1) = C(1, 3);
148 if (!f)
return false;
152 for (
unsigned int i = 0; i < 3; i++)
164 const CPose3D& newReferenceBase_)
179 const CPose2D& newReferenceBase)
192 const double ccos = cos(ang);
193 const double ssin = sin(ang);
196 const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos, 0., 0., 0., 1.};
229 "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
236 size_t N, std::vector<CVectorDouble>& outSamples)
const
243 std::vector<CVectorDouble> rndSamples;
246 outSamples.resize(N);
247 for (
unsigned int i = 0; i < N; i++)
249 outSamples[i].resize(3);
250 outSamples[i][0] =
mean.
x() + rndSamples[i][0];
251 outSamples[i][1] =
mean.
y() + rndSamples[i][1];
252 outSamples[i][2] =
mean.
phi() + rndSamples[i][2];
265 const double minMahalanobisDistToDrop)
285 this->
cov_inv = C1_inv + C2_inv;
312 const double ccos = ::cos(
mean.
phi());
313 const double ssin = ::sin(
mean.
phi());
317 -ccos, -ssin,
mean.
x() * ssin -
mean.
y() * ccos,
318 ssin, -ccos,
mean.
x() * ccos +
mean.
y() * ssin,
323 (H *
cov_inv * H.adjoint()).eval();
386 if (MU[0] == 0 && MU[1] == 0 && MU[2] == 0)
397 COV_.inv_fast(COV_inv);
411 out <<
"Mean: " <<
obj.mean <<
"\n";
412 out <<
"Inverse cov:\n" <<
obj.cov_inv <<
"\n";
500 df_dx.multiply_HCHt(OLD_COV,
cov);
501 df_du.multiply_HCHt(Ap_cov,
cov,
true);
531 res.inverseComposition(
a,
b);
double evaluateNormalizedPDF(const CPose2D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,...
void normalizePhi()
Forces "phi" to be in the range [-pi,pi];.
int void fclose(FILE *f)
An OS-independent version of fclose.
double mahalanobisDistanceTo(const CPosePDFGaussianInf &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
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,...
#define MRPT_END_WITH_CLEAN_UP(stuff)
void composeFrom(const CPose2D &A, const CPose2D &B)
Makes .
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
void operator+=(const CPose2D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
const double & phi() const
Get the phi angle of the 2D pose (in radians)
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
mrpt::math::TPoint2D operator+(const CPose2D &pose, const mrpt::math::TPoint2D &pnt)
Compose a 2D point from a new coordinate base given by a 2D pose.
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
GLsizei GLsizei GLuint * obj
CPose2D mean
The mean value.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
void drawGaussianMultivariateMany(VECTOR_OF_VECTORS &ret, size_t desiredSamples, const COVMATRIX &cov, const typename VECTOR_OF_VECTORS::value_type *mean=nullptr)
Generate a given number of multidimensional random samples according to a given covariance matrix.
TDATA getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
static void jacobiansPoseComposition(const CPose2D &x, const CPose2D &u, mrpt::math::CMatrixDouble33 &df_dx, mrpt::math::CMatrixDouble33 &df_du, const bool compute_df_dx=true, const bool compute_df_du=true)
This static method computes the pose composition Jacobians, with these formulas:
void rotateCov(const double ang)
Rotate the covariance matrix by replacing it by , where .
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of two points gauss.
#define ASSERT_(f)
Defines an assertion mechanism.
CPose2D operator-(const CPose2D &p)
Unary - operator: return the inverse pose "-p" (Note that is NOT the same than a pose with negative x...
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void wrapToPiInPlace(T &a)
Modifies the given angle to translate it into the ]-pi,pi] range.
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,...
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".
CPose2D mean
The mean value.
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
bool 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...
CPosePDFGaussianInf()
Default constructor (mean=all zeros, inverse covariance=all zeros -> so be careful!...
void inverseComposition(const CPosePDFGaussian &x, const CPosePDFGaussian &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jacobi...
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
double evaluatePDF(const CPose2D &x) const
Evaluates the PDF at a given point.
Declares a class that represents a Probability Density function (PDF) of a 3D pose as a Gaussian des...
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
#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.
virtual void getMean(TDATA &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
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 inverseComposition(const CPosePDFGaussianInf &x, const CPosePDFGaussianInf &ref)
Set , computing the mean using the "-" operator and the covariances through the corresponding Jacobi...
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
bool operator==(const CPoint< DERIVEDCLASS > &p1, const CPoint< DERIVEDCLASS > &p2)
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
MAT_C::Scalar multiply_HCHt_scalar(const VECTOR_H &H, const MAT_C &C)
r (a scalar) = H * C * H^t (with a vector H and a symmetric matrix C)
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
This base provides a set of functions for maths stuff.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
GLsizei const GLchar ** string
std::ostream & operator<<(std::ostream &o, const CPoint< DERIVEDCLASS > &p)
Dumps a point as a string [x,y] or [x,y,z]
A namespace of pseudo-random numbers generators of diferent distributions.
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.
#define MRPT_MAX_ALIGN_BYTES
CMatrixFixedNumeric< double, 3, 1 > CMatrixDouble31
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
GLubyte GLubyte GLubyte a
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 | |