Go to the documentation of this file.
64 :
mean(init_Mean),
cov(init_Cov)
74 for (
size_t i = 0; i < 3; i++)
76 const size_t ii = (i == 2) ? 3 : i;
77 for (
size_t j = 0; j < 3; j++)
79 const size_t jj = (j == 2) ? 3 : j;
102 q.rpy(OUT[2], OUT[1], OUT[0]);
115 q.rpy(
y[5],
y[4],
y[3]);
154 for (
int i = 0; i < 4; i++)
x[i] = o.
mean.
quat()[i];
158 jacobians::jacob_numeric_estimate(
x, ffff, Ax, o.
mean.
quat(), H);
159 cout <<
"num:" << endl << H << endl << endl;
165 cout <<
"lin:" << endl << J * NJ << endl << endl;
178 dr_dq_sub.multiply(dr_dq_sub_aux, dnorm_dq);
188 o.
cov.extractMatrix(3, 3, cov_Q);
189 o.
cov.extractMatrix(0, 0, cov_T);
190 o.
cov.extractMatrix(0, 3, cov_TQ);
197 this->
cov.insertMatrix(0, 0, cov_T);
201 cov_TR.multiply_ABt(cov_TQ, dr_dq_sub);
202 this->
cov.insertMatrix(0, 3, cov_TR);
203 this->
cov.insertMatrixTranspose(3, 0, cov_TR);
207 dr_dq_sub.multiply_HCHt(cov_Q, cov_r);
208 this->
cov.insertMatrix(3, 3, cov_r);
216 static const bool elements_do_wrapPI[6] = {
217 false,
false,
false,
true,
true,
true};
219 static const double dummy = 0;
224 y_mean[0], y_mean[1], y_mean[2], y_mean[3], y_mean[4], y_mean[5]);
253 if (
this == &o)
return;
268 cov.get_unsafe(0, 0) = C.get_unsafe(0, 0);
269 cov.get_unsafe(1, 1) = C.get_unsafe(1, 1);
270 cov.get_unsafe(3, 3) = C.get_unsafe(2, 2);
272 cov.get_unsafe(0, 1) =
cov.get_unsafe(1, 0) = C.get_unsafe(0, 1);
274 cov.get_unsafe(0, 3) =
cov.get_unsafe(3, 0) = C.get_unsafe(0, 2);
276 cov.get_unsafe(1, 3) =
cov.get_unsafe(3, 1) = C.get_unsafe(1, 2);
285 if (!f)
return false;
291 for (
unsigned int i = 0; i < 6; i++)
293 f,
"%e %e %e %e %e %e\n",
cov(i, 0),
cov(i, 1),
cov(i, 2),
304 const CPose3D& newReferenceBase)
319 df_du.multiply_HCHt(OLD_COV,
cov);
342 cov.saveToTextFile(
"__DEBUG_EXC_DUMP_drawSingleSample_COV.txt"););
349 size_t N, vector<CVectorDouble>& outSamples)
const
356 it != outSamples.end(); ++it)
358 (*it)[0] +=
mean.
x();
359 (*it)[1] +=
mean.
y();
360 (*it)[2] +=
mean.z();
426 out = p_zero - *
this;
444 df_dx.multiply_HCHt(OLD_COV,
cov);
560 for (
unsigned int i = 0; i <
cov.rows() - 1; i++)
561 for (
unsigned int j = i + 1; j <
cov.rows(); j++)
562 cov.get_unsafe(i, j) =
cov.get_unsafe(j, i);
576 for (
int i = 0; i < 6; i++)
578 if (COV_.get_unsafe(i, i) == 0)
580 if (MU.get_unsafe(i, 0) != 0)
581 return std::numeric_limits<double>::infinity();
583 COV_.get_unsafe(i, i) = 1;
589 return std::sqrt(MU.multiply_HtCH_scalar(COV_.inv()));
599 out <<
"Mean: " <<
obj.mean <<
"\n";
600 out <<
"Covariance:\n" <<
obj.cov <<
"\n";
610 out_cov.setSize(3, 3);
612 for (
int i = 0; i < 3; i++)
614 int a = i == 2 ? 3 : i;
615 for (
int j = i; j < 3; j++)
617 int b = j == 2 ? 3 : j;
618 double f =
cov.get_unsafe(
a,
b);
619 out_cov.set_unsafe(i, j, f);
620 out_cov.set_unsafe(j, i, f);
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.
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...
GLdouble GLdouble GLdouble GLdouble q
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)
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
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::CMatrixDouble33 cov
The 3x3 covariance matrix.
GLsizei GLsizei GLuint * obj
#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.
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,...
CPose3DPDFGaussian()
Default constructor.
Virtual base class for "archives": classes abstracting I/O streams.
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.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
void assureSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
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.
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 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)
CArrayNumeric is an array for numeric types supporting several mathematical operations (actually,...
double evaluatePDF(const CPose3D &x) const
Evaluates the PDF at a given point.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
#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.
CMatrixFixedNumeric< double, 6, 6 > CMatrixDouble66
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.
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)
double yaw() const
Get the YAW angle (in radians)
GLsizei const GLfloat * value
void operator-=(const CPose3DPDFGaussian &Ap)
Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean,...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
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.
void aux_posequat2poseypr(const CArrayDouble< 7 > &x, const double &dummy, CArrayDouble< 6 > &y)
mrpt::math::CMatrixDouble66 cov
The 6x6 covariance matrix.
CMatrixFixedNumeric< double, 6, 1 > CMatrixDouble61
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]
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.
This namespace provides a OS-independent interface to many useful functions: filenames manipulation,...
GLubyte GLubyte GLubyte a
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 | |