MRPT  2.0.1
CPosePDFGrid.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
13 #include <mrpt/poses/CPosePDF.h>
14 
15 namespace mrpt::poses
16 {
17 /** Declares a class that represents a Probability Distribution
18  * function (PDF) of a 2D pose (x,y,phi).
19  * This class implements that PDF using a 3D grid.
20  *
21  * \sa CPose2D, CPosePDF, CPose2DGridTemplate
22  * \ingroup poses_pdf_grp
23  */
24 class CPosePDFGrid : public CPosePDF, public CPose2DGridTemplate<double>
25 {
27 
28  protected:
29  public:
30  /** Constructor: Initializes a, uniform distribution over the whole given
31  * range.
32  */
34  double xMin = -1.0f, double xMax = 1.0f, double yMin = -1.0f,
35  double yMax = 1.0f, double resolutionXY = 0.5f,
36  double resolutionPhi = mrpt::DEG2RAD(180.0), double phiMin = -M_PI,
37  double phiMax = M_PI);
38 
39  /** Destructor */
40  ~CPosePDFGrid() override;
41 
42  /** Copy operator, translating if necesary (for example, between particles
43  * and gaussian representations) */
44  void copyFrom(const CPosePDF& o) override;
45 
46  /** Normalizes the PDF, such as all cells sum the unity. */
47  void normalize();
48  /** Assigns the same value to all the cells in the grid, so the sum 1. */
49  void uniformDistribution();
50 
51  void getMean(CPose2D& mean_pose) const override;
52 
53  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override;
54 
55  /** Save the contents of the 3D grid in one file, as a vertical
56  * concatenation of rectangular matrix for the different "PHI" discrete
57  * levels, and the size in X,Y,and PHI in another file named
58  * "<filename>_dims.txt". \return false on error */
59  bool saveToTextFile(const std::string& dataFile) const override;
60 
61  /** this = p (+) this. This can be used to convert a PDF from local
62  * coordinates to global, providing the point (newReferenceBase) from which
63  * "to project" the current pdf. Result PDF substituted the currently
64  * stored one in the object. */
65  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
66  /** Bayesian fusion of 2 densities (In the grid representation this becomes
67  * a pointwise multiplication) */
68  void bayesianFusion(
69  const CPosePDF& p1, const CPosePDF& p2,
70  const double minMahalanobisDistToDrop = 0) override;
71  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF */
72  void inverse(CPosePDF& o) const override;
73  /** Draws a single sample from the distribution (WARNING: weights are
74  * assumed to be normalized!) */
75  void drawSingleSample(CPose2D& outPart) const override;
76  /** Draws a number of samples from the distribution, and saves as a list of
77  * 1x3 vectors, where each row contains a (x,y,phi) datum. */
78  void drawManySamples(
79  size_t N,
80  std::vector<mrpt::math::CVectorDouble>& outSamples) const override;
81 
82 }; // End of class def.
83 } // namespace mrpt::poses
void getMean(CPose2D &mean_pose) const override
CPosePDFGrid(double xMin=-1.0f, double xMax=1.0f, double yMin=-1.0f, double yMax=1.0f, double resolutionXY=0.5f, double resolutionPhi=mrpt::DEG2RAD(180.0), double phiMin=-M_PI, double phiMax=M_PI)
Constructor: Initializes a, uniform distribution over the whole given range.
void uniformDistribution()
Assigns the same value to all the cells in the grid, so the sum 1.
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
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 drawSingleSample(CPose2D &outPart) const override
Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) ...
~CPosePDFGrid() override
Destructor.
constexpr double DEG2RAD(const double x)
Degrees to radians.
void copyFrom(const CPosePDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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, where each row contains a (x,y,phi) datum.
bool saveToTextFile(const std::string &dataFile) const override
Save the contents of the 3D grid in one file, as a vertical concatenation of rectangular matrix for t...
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
void inverse(CPosePDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
void bayesianFusion(const CPosePDF &p1, const CPosePDF &p2, const double minMahalanobisDistToDrop=0) override
Bayesian fusion of 2 densities (In the grid representation this becomes a pointwise multiplication) ...
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Declares a class that represents a Probability Distribution function (PDF) of a 2D pose (x...
Definition: CPosePDFGrid.h:24
void normalize()
Normalizes the PDF, such as all cells sum the unity.
This is a template class for storing a 3D (2D+heading) grid containing any kind of data...



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020