MRPT  1.9.9
CPose3DPDFGrid.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/bits_math.h> // DEG2RAD()
12 #include <mrpt/math/TPose3D.h>
14 #include <mrpt/poses/CPose3DPDF.h>
15 
16 namespace mrpt::poses
17 {
18 /** Declares a class that represents a Probability Distribution
19  * function (PDF) of a SE(3) pose (x,y,z, yaw, pitch, roll), in
20  * the form of a 6-dimensional grid of "voxels".
21  *
22  * \sa CPose3D, CPose3DPDF, CPose3DGridTemplate
23  * \ingroup poses_pdf_grp
24  */
25 class CPose3DPDFGrid : public CPose3DPDF, public CPose3DGridTemplate<double>
26 {
28 
29  protected:
30  public:
31  /** Constructor: Initializes a, uniform distribution over the whole given
32  * range, given by a "rectangular" (6D) bounding box.
33  */
36  mrpt::math::TPose3D(-1., -1., -1., -M_PI, -.5 * M_PI, -.5 * M_PI),
38  mrpt::math::TPose3D(1., 1., 1., M_PI, .5 * M_PI, .5 * M_PI),
39  double resolution_XYZ = 0.10,
40  double resolution_YPR = mrpt::DEG2RAD(10.0));
41 
42  /** Destructor */
43  ~CPose3DPDFGrid() override = default;
44 
45  /** Copy operator, translating if necesary (for example, between
46  * particles and gaussian representations) */
47  void copyFrom(const CPose3DPDF& o) override;
48 
49  /** Normalizes the PDF, such as all voxels sum the unity. */
50  void normalize();
51 
52  /** Assigns the same value to all the cells in the grid, so the sum 1 */
53  void uniformDistribution();
54 
55  void getMean(CPose3D& mean_pose) const override;
56 
57  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override;
58 
59  /** Save the contents of the 3D grid in one file, as a concatenation of
60  * (X,Y) slices. The size in X,Y,and the values for Z,yaw, pitch, roll, PHI
61  * are stored in another file named `<filename>_dims.txt`
62  * \return false on error */
63  bool saveToTextFile(const std::string& dataFile) const override;
64 
65  // See base docs
66  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
67  // See base docs. Not implemented in this class.
68  void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2) override;
69 
70  // See base docs. Not implemented in this class.
71  void inverse(CPose3DPDF& o) const override;
72 
73  /** Draws a single sample from the distribution.
74  * \note Precondition: voxel weights are assumed to be normalized. */
75  void drawSingleSample(CPose3D& outPart) const override;
76 
77  /** Draws a number of samples from the distribution, and saves as a list
78  * of 1x6 vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum.
79  */
80  void drawManySamples(
81  size_t N,
82  std::vector<mrpt::math::CVectorDouble>& outSamples) const override;
83 
84 }; // End of class def.
85 } // namespace mrpt::poses
~CPose3DPDFGrid() override=default
Destructor.
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
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, where each row contains a (x,y,z,yaw,pitch,roll) datum.
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
double DEG2RAD(const double x)
Degrees to radians.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
This is a template class for storing a 6-dimensional grid, with components corresponding to Euler ang...
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Returns an estimate of the pose covariance matrix (STATE_LENxSTATE_LEN cov matrix) and the mean...
void uniformDistribution()
Assigns the same value to all the cells in the grid, so the sum 1.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
void getMean(CPose3D &mean_pose) const override
bool saveToTextFile(const std::string &dataFile) const override
Save the contents of the 3D grid in one file, as a concatenation of (X,Y) slices. ...
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:23
Declares a class that represents a Probability Distribution function (PDF) of a SE(3) pose (x...
const auto bb_max
const auto bb_min
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
void normalize()
Normalizes the PDF, such as all voxels sum the unity.
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently on...
CPose3DPDFGrid(const mrpt::math::TPose3D &bb_min=mrpt::math::TPose3D(-1., -1., -1., -M_PI, -.5 *M_PI, -.5 *M_PI), const mrpt::math::TPose3D &bb_max=mrpt::math::TPose3D(1., 1., 1., M_PI,.5 *M_PI,.5 *M_PI), double resolution_XYZ=0.10, double resolution_YPR=mrpt::DEG2RAD(10.0))
Constructor: Initializes a, uniform distribution over the whole given range, given by a "rectangular"...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019