Go to the documentation of this file.
25 #include <Eigen/Dense>
45 :
mean(init_Mean), cov_inv(init_CovInv)
53 :
mean(init_Mean), cov_inv()
72 in >> p.
x >> p.
y >> p.
phi;
116 if (
this == &o)
return;
145 cov_inv(0, 0) = ptr->cov_inv(0, 0);
146 cov_inv(1, 1) = ptr->cov_inv(1, 1);
147 cov_inv(2, 2) = ptr->cov_inv(3, 3);
159 o_cov(0, 0) = C(0, 0);
160 o_cov(1, 1) = C(1, 1);
161 o_cov(2, 2) = C(3, 3);
162 o_cov(0, 1) = o_cov(1, 0) = C(0, 1);
163 o_cov(0, 2) = o_cov(2, 0) = C(0, 3);
164 o_cov(1, 2) = o_cov(2, 1) = C(1, 3);
176 if (!f)
return false;
180 for (
unsigned int i = 0; i < 3; i++)
192 const CPose3D& newReferenceBase_)
207 const CPose2D& newReferenceBase)
220 const double ccos = cos(ang);
221 const double ssin = sin(ang);
223 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
224 const double rot_vals[] = {ccos, -ssin, 0., ssin, ccos, 0., 0., 0., 1.};
244 outPart.
x(
mean.
x() + v[0]);
245 outPart.
y(
mean.
y() + v[1]);
252 "__DEBUG_EXC_DUMP_drawSingleSample_COV_INV.txt"););
256 size_t N, std::vector<CVectorDouble>& outSamples)
const
262 std::vector<CVectorDouble> rndSamples;
265 outSamples.resize(N);
266 for (
size_t i = 0; i < N; i++)
268 outSamples[i].resize(3);
269 outSamples[i][0] =
mean.
x() + rndSamples[i][0];
270 outSamples[i][1] =
mean.
y() + rndSamples[i][1];
271 outSamples[i][2] =
mean.
phi() + rndSamples[i][2];
284 [[maybe_unused]]
const double minMahalanobisDistToDrop)
300 this->
cov_inv = C1_inv + C2_inv;
306 this->
mean.
x(x(0, 0));
307 this->
mean.
y(x(1, 0));
326 const double ccos = ::cos(
mean.
phi());
327 const double ssin = ::sin(
mean.
phi());
330 alignas(MRPT_MAX_STATIC_ALIGN_BYTES)
const double H_values[] = {
331 -ccos, -ssin,
mean.
x() * ssin -
mean.
y() * ccos,
332 ssin, -ccos,
mean.
x() * ccos +
mean.
y() * ssin,
337 out->cov_inv.asEigen().noalias() =
399 if (MU[0] == 0 && MU[1] == 0 && MU[2] == 0)
421 out <<
"Mean: " << obj.
mean <<
"\n";
422 out <<
"Inverse cov:\n" << obj.
cov_inv <<
"\n";
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample,...
double evaluateNormalizedPDF(const CPose2D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,...
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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.
double phi
Orientation (rads)
double phi() const
Get the phi angle of the 2D pose (in radians)
Virtual base class for "schematic archives" (JSON, XML,...)
double x() const
Common members of all points & poses classes.
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 .
#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.
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...
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.
CPose2D mean
The mean value.
#define SCHEMA_DESERIALIZE_DATATYPE_VERSION()
For use inside serializeFrom(CSchemeArchiveBase) methods.
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
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.
std::ostream & operator<<(std::ostream &o, const CPoint2D &p)
Dumps a point as a string (x,y)
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.
void readTo(CSerializable &obj)
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.
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Derived inverse() const
Returns the inverse of a general matrix using LU.
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)
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 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.
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).
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...
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
A Probability Density function (PDF) of a 2D pose as a Gaussian with a mean and the inverse of the c...
type_value getMeanVal() const
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
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.
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.
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
void drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution.
virtual void getMean(type_value &mean_point) const =0
Returns the mean, or mathematical expectation of the probability density distribution (PDF).
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
This base provides a set of functions for maths stuff.
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
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.
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.
CMatrixFixed< double, 3, 1 > CMatrixDouble31
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
#define SCHEMA_SERIALIZE_DATATYPE_VERSION(ser_version)
For use inside all serializeTo(CSchemeArchiveBase) methods.
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 2.0.3 at Fri May 29 13:06:46 UTC 2020 | |