Go to the documentation of this file.
23 #include <Eigen/Dense>
52 :
mean(init_Mean), cov_inv(init_Cov)
60 :
mean(init_Mean), cov_inv()
107 if (
this == &o)
return;
136 cov_inv(0, 0) = ptr->cov_inv(0, 0);
137 cov_inv(1, 1) = ptr->cov_inv(1, 1);
138 cov_inv(3, 3) = ptr->cov_inv(2, 2);
158 if (!f)
return false;
164 for (
unsigned int i = 0; i < 6; i++)
177 const CPose3D& newReferenceBase)
183 a.changeCoordinatesReference(newReferenceBase);
193 [maybe_unused]]
CPose3D& outPart)
const
202 outPart.setFromValues(
207 "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
214 size_t N, vector<CVectorDouble>& outSamples)
const
222 for (
auto& outSample : outSamples)
224 outSample[0] +=
mean.
x();
225 outSample[1] +=
mean.
y();
226 outSample[2] +=
mean.z();
318 [maybe_unused]]
const CPose3D& x)
const
327 [maybe_unused]]
const CPose3D& x)
const
358 for (
int i = 0; i < 6; i++)
363 return std::numeric_limits<double>::infinity();
381 out <<
"Mean: " << obj.
mean <<
"\n";
382 out <<
"Inverse cov:\n" << obj.
cov_inv <<
"\n";
394 for (
int i = 0; i < 3; i++)
396 int a = i == 2 ? 3 : i;
397 for (
int j = i; j < 3; j++)
399 int b = j == 2 ? 3 : j;
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample,...
CMatrixFixed< double, 6, 1 > CMatrixDouble61
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 .
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)
virtual std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const =0
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean,...
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 ...
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
double x() const
Common members of all points & poses classes.
#define MRPT_END_WITH_CLEAN_UP(stuff)
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
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.
void operator-=(const CPose3DPDFGaussianInf &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean,...
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.
mrpt::vision::TStereoCalibResults out
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.
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
#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...
Virtual base class for "archives": classes abstracting I/O streams.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
constexpr size_type rows() const
Number of rows in the matrix.
void operator+=(const CPose3D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
CPose3D mean
The mean value.
A compile-time fixed-size numeric matrix container.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
MAT_C::Scalar multiply_HtCH_scalar(const VECTOR_H &H, const MAT_C &C)
r (scalar) = H^t*C*H (H: column vector, C: symmetric 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)
void multiply_HCHt(const MAT_H &H, const MAT_C &C, MAT_R &R, bool accumResultInOutput=false)
R = H * C * H^t.
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...
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...
void saveToTextFile(const std::string &file, mrpt::math::TMatrixTextFileFormat fileFormat=mrpt::math::MATRIX_FORMAT_ENG, bool appendMRPTHeader=false, const std::string &userHeader=std::string()) const
Saves the vector/matrix to a file compatible with MATLAB/Octave text format.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to 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.
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.
CPose3DPDFGaussianInf()
Default constructor - mean: all zeros, inverse covariance=all zeros -> so be careful!
double yaw() const
Get the YAW angle (in radians)
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
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.
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
A namespace of pseudo-random numbers generators of diferent distributions.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
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 enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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 2.0.3 at Fri May 15 23:51:15 UTC 2020 | |