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