MRPT  2.0.4
CPose3DPDFSOG.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 
12 #include <mrpt/poses/CPose3DPDF.h>
14 
15 namespace mrpt::poses
16 {
17 /** Declares a class that represents a Probability Density function (PDF) of a
18  * 3D(6D) pose \f$ p(\mathbf{x}) = [x ~ y ~ z ~ yaw ~ pitch ~ roll]^t \f$.
19  * This class implements that PDF as the following multi-modal Gaussian
20  * distribution:
21  *
22  * \f$ p(\mathbf{x}) = \sum\limits_{i=1}^N \omega^i \mathcal{N}( \mathbf{x} ;
23  * \bar{\mathbf{x}}^i, \mathbf{\Sigma}^i ) \f$
24  *
25  * Where the number of modes N is the size of CPose3DPDFSOG::m_modes. Angles
26  * are always in radians.
27  *
28  * See mrpt::poses::CPose3DPDF for more details.
29  * \ingroup poses_pdf_grp
30  * \sa CPose3DPDF
31  */
32 class CPose3DPDFSOG : public CPose3DPDF
33 {
35 
36  public:
37  /** The struct for each mode:
38  */
40  {
41  TGaussianMode() : val() {}
43 
44  /** The log-weight
45  */
46  double log_w{0};
47  };
48 
49  using TModesList = std::vector<
51  using const_iterator = TModesList::const_iterator;
52  using iterator = TModesList::iterator;
53 
54  protected:
55  /** Assures the symmetry of the covariance matrix (eventually certain
56  * operations in the math-coprocessor lead to non-symmetric matrixes!)
57  */
58  void enforceCovSymmetry();
59 
60  /** Access directly to this array for modify the modes as desired.
61  * Note that no weight can be zero!!
62  * We must use pointers to satisfy the mem-alignment of the matrixes
63  */
65 
66  public:
67  /** Default constructor
68  * \param nModes The initial size of CPose3DPDFSOG::m_modes
69  */
70  CPose3DPDFSOG(size_t nModes = 1);
71 
72  /** Clear all the gaussian modes */
73  void clear();
74  /** Set the number of SOG modes */
75  void resize(const size_t N);
76  /** Return the number of Gaussian modes. */
77  size_t size() const { return m_modes.size(); }
78  /** Return whether there is any Gaussian mode. */
79  bool empty() const { return m_modes.empty(); }
80  iterator begin() { return m_modes.begin(); }
81  iterator end() { return m_modes.end(); }
82  const_iterator begin() const { return m_modes.begin(); }
83  const_iterator end() const { return m_modes.end(); }
84 
85  void getMean(CPose3D& mean_pose) const override;
86  std::tuple<cov_mat_t, type_value> getCovarianceAndMean() const override;
87 
88  /** Normalize the weights in m_modes such as the maximum log-weight is 0. */
89  void normalizeWeights();
90  /** Return the Gaussian mode with the highest likelihood (or an empty
91  * Gaussian if there are no modes in this SOG) */
92  void getMostLikelyMode(CPose3DPDFGaussian& outVal) const;
93 
94  /** Copy operator, translating if necesary (for example, between particles
95  * and gaussian representations) */
96  void copyFrom(const CPose3DPDF& o) override;
97 
98  /** Save the density to a text file, with the following format:
99  * There is one row per Gaussian "mode", and each row contains 10
100  * elements:
101  * - w (The linear weight)
102  * - x_mean (gaussian mean value)
103  * - y_mean (gaussian mean value)
104  * - x_mean (gaussian mean value)
105  * - yaw_mean (gaussian mean value, in radians)
106  * - pitch_mean (gaussian mean value, in radians)
107  * - roll_mean (gaussian mean value, in radians)
108  * - C11,C22,C33,C44,C55,C66 (Covariance elements)
109  * - C12,C13,C14,C15,C16 (Covariance elements)
110  * - C23,C24,C25,C25 (Covariance elements)
111  * - C34,C35,C36 (Covariance elements)
112  * - C45,C46 (Covariance elements)
113  * - C56 (Covariance elements)
114  *
115  */
116  bool saveToTextFile(const std::string& file) const override;
117 
118  /** this = p (+) this. This can be used to convert a PDF from local
119  * coordinates to global, providing the point (newReferenceBase) from which
120  * "to project" the current pdf. Result PDF substituted the currently
121  * stored one in the object. */
122  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
123 
124  /** Bayesian fusion of two pose distributions, then save the result in this
125  * object (WARNING: Currently p1 must be a mrpt::poses::CPose3DPDFSOG object
126  * and p2 a mrpt::poses::CPose3DPDFSOG object) */
127  void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2) override;
128 
129  /** Draws a single sample from the distribution */
130  void drawSingleSample(CPose3D& outPart) const override;
131  /** Draws a number of samples from the distribution, and saves as a list of
132  * 1x6 vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum */
133  void drawManySamples(
134  size_t N,
135  std::vector<mrpt::math::CVectorDouble>& outSamples) const override;
136  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF */
137  void inverse(CPose3DPDF& o) const override;
138 
139  /** Append the Gaussian modes from "o" to the current set of modes of "this"
140  * density */
141  void appendFrom(const CPose3DPDFSOG& o);
142 
143 }; // End of class def.
144 } // namespace mrpt::poses
CPose3DPDFSOG(size_t nModes=1)
Default constructor.
std::vector< TGaussianMode, mrpt::aligned_allocator_cpp11< TGaussianMode > > TModesList
Definition: CPose3DPDFSOG.h:50
void enforceCovSymmetry()
Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor ...
void getMostLikelyMode(CPose3DPDFGaussian &outVal) const
Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in t...
void appendFrom(const CPose3DPDFSOG &o)
Append the Gaussian modes from "o" to the current set of modes of "this" density. ...
Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose ...
Definition: CPose3DPDFSOG.h:32
void getMean(CPose3D &mean_pose) const override
void normalizeWeights()
Normalize the weights in m_modes such as the maximum log-weight is 0.
void inverse(CPose3DPDF &o) const override
Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF.
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.
TModesList m_modes
Access directly to this array for modify the modes as desired.
Definition: CPose3DPDFSOG.h:64
bool saveToTextFile(const std::string &file) const override
Save the density to a text file, with the following format: There is one row per Gaussian "mode"...
void changeCoordinatesReference(const CPose3D &newReferenceBase) override
this = p (+) this.
Aligned allocator that is compatible with C++11.
bool empty() const
Return whether there is any Gaussian mode.
Definition: CPose3DPDFSOG.h:79
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TModesList::const_iterator const_iterator
Definition: CPose3DPDFSOG.h:51
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
void clear()
Clear all the gaussian modes.
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...
TModesList::iterator iterator
Definition: CPose3DPDFSOG.h:52
const_iterator end() const
Definition: CPose3DPDFSOG.h:83
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
void bayesianFusion(const CPose3DPDF &p1, const CPose3DPDF &p2) override
Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1...
size_t size() const
Return the number of Gaussian modes.
Definition: CPose3DPDFSOG.h:77
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
const_iterator begin() const
Definition: CPose3DPDFSOG.h:82
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
void copyFrom(const CPose3DPDF &o) override
Copy operator, translating if necesary (for example, between particles and gaussian representations) ...
void resize(const size_t N)
Set the number of SOG modes.



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020