MRPT  2.0.4
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-2020, 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> // .0_deg
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
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:152
mrpt::poses::CPose3DPDFGrid::normalize
void normalize()
Normalizes the PDF, such as all voxels sum the unity.
Definition: CPose3DPDFGrid.cpp:182
mrpt::poses::CPose3DPDFGrid::uniformDistribution
void uniformDistribution()
Assigns the same value to all the cells in the grid, so the sum 1.
Definition: CPose3DPDFGrid.cpp:196
mrpt::poses::CPose3DPDF
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually).
Definition: CPose3DPDF.h:39
mrpt::poses::CPose3DPDFGrid::getCovarianceAndMean
std::tuple< cov_mat_t, type_value > getCovarianceAndMean() const override
Definition: CPose3DPDFGrid.cpp:68
mrpt::poses::CPose3DPDFGrid::getMean
void getMean(CPose3D &mean_pose) const override
Definition: CPose3DPDFGrid.cpp:45
mrpt::poses::CPose3DPDFGrid
Declares a class that represents a Probability Distribution function (PDF) of a SE(3) pose (x,...
Definition: CPose3DPDFGrid.h:25
mrpt::poses::CPose3DPDFGrid::drawSingleSample
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
Definition: CPose3DPDFGrid.cpp:170
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::poses::CPose3DPDFGrid::bayesianFusion
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...
Definition: CPose3DPDFGrid.cpp:158
bb_max
const auto bb_max
Definition: CPose3DPDFGrid_unittest.cpp:25
bb_min
const auto bb_min
Definition: CPose3DPDFGrid_unittest.cpp:23
mrpt::poses::CPose3DGridTemplate
This is a template class for storing a 6-dimensional grid, with components corresponding to Euler ang...
Definition: CPose3DGridTemplate.h:23
mrpt::poses::CPose3DPDFGrid::saveToTextFile
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.
Definition: CPose3DPDFGrid.cpp:145
TPose3D.h
mrpt::poses::CPose3DPDFGrid::CPose3DPDFGrid
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"...
Definition: CPose3DPDFGrid.cpp:29
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::math::TPose3D
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
Definition: TPose3D.h:24
mrpt::poses::CPose3DPDFGrid::drawManySamples
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,...
Definition: CPose3DPDFGrid.cpp:175
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
CPose3DGridTemplate.h
bits_math.h
mrpt::poses::CPose3DPDFGrid::~CPose3DPDFGrid
~CPose3DPDFGrid() override=default
Destructor.
mrpt::poses::CPose3DPDFGrid::changeCoordinatesReference
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
Definition: CPose3DPDFGrid.cpp:152
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::poses::CPose3DPDFGrid::copyFrom
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations)
Definition: CPose3DPDFGrid.cpp:38
CPose3DPDF.h
mrpt::poses::CPose3DPDFGrid::inverse
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
Definition: CPose3DPDFGrid.cpp:165



Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sat Jun 27 14:00:59 UTC 2020