Go to the documentation of this file.
52 :
mean(init_Mean), cov_inv(init_Cov)
60 :
mean(init_Mean), cov_inv()
107 if (
this == &o)
return;
158 if (!f)
return false;
164 for (
unsigned int i = 0; i < 6; i++)
177 const CPose3D& newReferenceBase)
183 a.changeCoordinatesReference(newReferenceBase);
209 "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
216 size_t N, vector<CVectorDouble>& outSamples)
const
226 it != outSamples.end(); ++it)
228 (*it)[0] +=
mean.
x();
229 (*it)[1] +=
mean.
y();
230 (*it)[2] +=
mean.z();
281 df_dx.multiply_HCHt(OLD_COV_INV,
cov_inv);
345 for (
int i = 0; i <
cov_inv.rows() - 1; i++)
346 for (
int j = i + 1; j <
cov_inv.rows(); j++)
364 for (
int i = 0; i < 6; i++)
366 if (COV_.get_unsafe(i, i) == 0)
368 if (MU.get_unsafe(i, 0) != 0)
369 return std::numeric_limits<double>::infinity();
371 COV_.get_unsafe(i, i) = 1;
377 return std::sqrt(MU.multiply_HtCH_scalar(COV_.inv()));
387 out <<
"Mean: " <<
obj.mean <<
"\n";
388 out <<
"Inverse cov:\n" <<
obj.cov_inv <<
"\n";
398 out_cov.setSize(3, 3);
400 for (
int i = 0; i < 3; i++)
402 int a = i == 2 ? 3 : i;
403 for (
int j = i; j < 3; j++)
405 int b = j == 2 ? 3 : j;
407 out_cov.set_unsafe(i, j, f);
408 out_cov.set_unsafe(j, i, f);
double evaluatePDF(const CPose3D &x) const
Evaluates the PDF at a given point.
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
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,...
int void fclose(FILE *f)
An OS-independent version of fclose.
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
static void jacobiansPoseComposition(const CPose3D &x, const CPose3D &u, mrpt::math::CMatrixDouble66 &df_dx, mrpt::math::CMatrixDouble66 &df_du)
This static method computes the pose composition Jacobians.
mrpt::math::CMatrixDouble66 cov_inv
The inverse of the 6x6 covariance matrix.
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
double pitch() const
Get the PITCH angle (in radians)
bool saveToTextFile(const std::string &file) const override
Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in ...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction.
void setFromValues(const double x0, const double y0, const double z0, const double yaw=0, const double pitch=0, const double roll=0)
Set the pose from a 3D position (meters) and yaw/pitch/roll angles (radians) - This method recomputes...
#define MRPT_END_WITH_CLEAN_UP(stuff)
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.
GLsizei GLsizei GLuint * obj
void operator-=(const CPose3DPDFGaussianInf &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean,...
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.
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.
#define THROW_EXCEPTION(msg)
#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...
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.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
void operator+=(const CPose3D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
CPose3D mean
The mean value.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
mrpt::math::CMatrixDouble33 cov_inv
The inverse of the 3x3 covariance matrix (the "information" matrix)
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
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.
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...
double roll() const
Get the ROLL angle (in radians)
double evaluateNormalizedPDF(const CPose3D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
#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.
A numeric matrix of compile-time fixed size.
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.
#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)
CPose3DPDFGaussianInf()
Default constructor - mean: all zeros, inverse covariance=all zeros -> so be careful!
double yaw() const
Get the YAW angle (in radians)
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
double mahalanobisDistanceTo(const CPose3DPDFGaussianInf &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion...
This base provides a set of functions for maths stuff.
CMatrixFixedNumeric< double, 6, 1 > CMatrixDouble61
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
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.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two points gauss.
void getInvCovSubmatrix2D(mrpt::math::CMatrixDouble &out_cov) const
Returns a 3x3 matrix with submatrix of the inverse covariance for the variables (x,...
CPose3D mean
The mean value.
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
GLubyte GLubyte GLubyte a
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 1x6 vectors,...
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 | |