Go to the documentation of this file.
21 #include <Eigen/Dense>
63 :
mean(init_Mean),
cov(init_Cov)
73 for (
size_t i = 0; i < 3; i++)
75 const size_t ii = (i == 2) ? 3 : i;
76 for (
size_t j = 0; j < 3; j++)
78 const size_t jj = (j == 2) ? 3 : j;
101 q.rpy(OUT[2], OUT[1], OUT[0]);
114 q.
rpy(y[5], y[4], y[3]);
153 for (
int i = 0; i < 4; i++) x[i] = o.
mean.
quat()[i];
157 jacobians::jacob_numeric_estimate(x, ffff, Ax, o.
mean.
quat(), H);
158 cout <<
"num:" << endl << H << endl << endl;
164 cout <<
"lin:" << endl << J * NJ << endl << endl;
177 dr_dq_sub.
asEigen() = dr_dq_sub_aux * dnorm_dq;
197 this->
cov.
block<3, 3>(0, 3) = cov_TR;
198 this->
cov.
block<3, 3>(3, 0) = cov_TR.transpose();
210 static const bool elements_do_wrapPI[6] = {
211 false,
false,
false,
true,
true,
true};
213 const double dummy = 0;
218 y_mean[0], y_mean[1], y_mean[2], y_mean[3], y_mean[4], y_mean[5]);
247 if (
this == &o)
return;
266 cov(0, 1) =
cov(1, 0) = C(0, 1);
268 cov(0, 3) =
cov(3, 0) = C(0, 2);
270 cov(1, 3) =
cov(3, 1) = C(1, 2);
279 if (!f)
return false;
285 for (
unsigned int i = 0; i < 6; i++)
287 f,
"%e %e %e %e %e %e\n",
cov(i, 0),
cov(i, 1),
cov(i, 2),
298 const CPose3D& newReferenceBase)
343 size_t N, vector<CVectorDouble>& outSamples)
const
349 for (
auto& outSample : outSamples)
351 outSample[0] +=
mean.
x();
352 outSample[1] +=
mean.
y();
353 outSample[2] +=
mean.z();
418 out = p_zero - *
this;
526 [maybe_unused]]
const CPose3D& x)
const
548 for (
int i = 0; i <
cov.
rows() - 1; i++)
549 for (
int j = i + 1; j <
cov.
rows(); j++)
cov(i, j) =
cov(j, i);
563 for (
int i = 0; i < 6; i++)
568 return std::numeric_limits<double>::infinity();
586 out <<
"Mean: " << obj.
mean <<
"\n";
587 out <<
"Covariance:\n" << obj.
cov <<
"\n";
599 for (
int i = 0; i < 3; i++)
601 int a = i == 2 ? 3 : i;
602 for (
int j = i; j < 3; j++)
604 int b = j == 2 ? 3 : j;
605 double f =
cov(a, b);
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
Declares a class that represents a Probability Density function (PDF) of a 3D pose .
void assign(const std::size_t N, const Scalar value)
void normalize()
Normalize this quaternion, so its norm becomes the unitity.
int void fclose(FILE *f)
An OS-independent version of fclose.
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.
void composeFrom(const CPose3D &A, const CPose3D &B)
Makes "this = A (+) B"; this method is slightly more efficient than "this= A + B;" since it avoids th...
virtual const mrpt::rtti::TRuntimeClassId * GetRuntimeClass() const override
Returns information about the class of an object in runtime.
std::string asString() const
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,...
void operator+=(const CPose3D &Ap)
Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matri...
bool USE_SUT_QUAT2EULER_CONVERSION_value
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.
static void aux_posequat2poseypr(const CVectorFixedDouble< 7 > &x, [[maybe_unused]] const double &dummy, CVectorFixedDouble< 6 > &y)
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)
double mean(const CONTAINER &v)
Computes the mean value of a vector.
CMatrixFixed< double, 6, 6 > CMatrixDouble66
void rpy(T &roll, T &pitch, T &yaw) const
Return the yaw, pitch & roll angles associated to quaternion.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
mrpt::math::CMatrixDouble33 cov
The 3x3 covariance matrix.
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 enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
void resize(std::size_t N, bool zeroNewElements=false)
CPose3DPDFGaussian()
Default constructor.
Virtual base class for "archives": classes abstracting I/O streams.
constexpr size_type rows() const
Number of rows in the matrix.
Derived inverse_LLt() const
Returns the inverse of a symmetric matrix using LLt.
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,...
void USE_SUT_QUAT2EULER_CONVERSION(bool value)
If set to true (false), a Scaled Unscented Transform is used instead of a linear approximation with J...
double mahalanobisDistanceTo(const CPose3DPDFGaussian &theOther)
Computes the Mahalanobis distance between the centers of two Gaussians.
mrpt::math::CQuaternionDouble & quat()
Read/Write access to the quaternion representing the 3D rotation.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
double evaluateNormalizedPDF(const CPose3D &x) const
Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,...
mrpt::math::CMatrixDouble77 cov
The 7x7 covariance matrix.
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two points gauss.
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
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 drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
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 getCovSubmatrix2D(mrpt::math::CMatrixDouble &out_cov) const
Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only.
void deserializeSymmetricMatrixFrom(MAT &m, mrpt::serialization::CArchive &in)
Binary serialization of symmetric matrices, saving the space of duplicated values.
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 normalizationJacobian(MATRIXLIKE &J) const
Calculate the 4x4 Jacobian of the normalization operation of this quaternion.
double roll() const
Get the ROLL angle (in radians)
void insertMatrix(const int row_start, const int col_start, const OTHERMATVEC &submat)
Copies the given input submatrix/vector into this matrix/vector, starting at the given top-left coord...
auto block(int start_row, int start_col)
non-const block(): Returns an Eigen::Block reference to the block
double evaluatePDF(const CPose3D &x) const
Evaluates the PDF at a given point.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
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.
CMatrixFixed< Scalar, BLOCK_ROWS, BLOCK_COLS > blockCopy(int start_row=0, int start_col=0) const
const blockCopy(): Returns a copy of the given block
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.
A quaternion, which can represent a 3D rotation as pair , with a real part "r" and a 3D vector ,...
#define CLASS_ID(T)
Access to runtime class ID for a defined class name.
CPose3DQuat mean
The mean value.
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.
double yaw() const
Get the YAW angle (in radians)
bool operator==(const CPoint< DERIVEDCLASS, DIM > &p1, const CPoint< DERIVEDCLASS, DIM > &p2)
void operator-=(const CPose3DPDFGaussian &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean,...
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.
void rpy_and_jacobian(T &roll, T &pitch, T &yaw, MATRIXLIKE *out_dr_dq=nullptr, bool resize_out_dr_dq_to3x4=true) const
Return the yaw, pitch & roll angles associated to quaternion, and (optionally) the 3x4 Jacobian of th...
A namespace of pseudo-random numbers generators of diferent distributions.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
CPose3D mean
The mean value.
Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020 | |