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::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(), log_w(0) {}
43 
44  /** The log-weight
45  */
46  double log_w;
47  };
48 
49  using TModesList =
50  std::vector<TGaussianMode,
54 
55  protected:
56  /** Assures the symmetry of the covariance matrix (eventually certain
57  * operations in the math-coprocessor lead to non-symmetric matrixes!)
58  */
59  void assureSymmetry();
60 
61  /** Access directly to this array for modify the modes as desired.
62  * Note that no weight can be zero!!
63  * We must use pointers to satisfy the mem-alignment of the matrixes
64  */
66 
67  public:
68  /** Default constructor
69  * \param nModes The initial size of CPose3DPDFSOG::m_modes
70  */
71  CPose3DPDFSOG(size_t nModes = 1);
72 
73  /** Clear all the gaussian modes */
74  void clear();
75  /** Set the number of SOG modes */
76  void resize(const size_t N);
77  /** Return the number of Gaussian modes. */
78  size_t size() const { return m_modes.size(); }
79  /** Return whether there is any Gaussian mode. */
80  bool empty() const { return m_modes.empty(); }
81  iterator begin() { return m_modes.begin(); }
82  iterator end() { return m_modes.end(); }
83  const_iterator begin() const { return m_modes.begin(); }
84  const_iterator end() const { return m_modes.end(); }
85  /** Returns an estimate of the pose, (the mean, or mathematical expectation
86  * of the PDF), computed as a weighted average over all m_particles. \sa
87  * getCovariance */
88  void getMean(CPose3D& mean_pose) const override;
89  /** Returns an estimate of the pose covariance matrix (6x6 cov matrix) and
90  * the mean, both at once. \sa getMean */
92  mrpt::math::CMatrixDouble66& cov, CPose3D& mean_point) const override;
93  /** Normalize the weights in m_modes such as the maximum log-weight is 0. */
94  void normalizeWeights();
95  /** Return the Gaussian mode with the highest likelihood (or an empty
96  * Gaussian if there are no modes in this SOG) */
97  void getMostLikelyMode(CPose3DPDFGaussian& outVal) const;
98 
99  /** Copy operator, translating if necesary (for example, between particles
100  * and gaussian representations) */
101  void copyFrom(const CPose3DPDF& o) override;
102 
103  /** Save the density to a text file, with the following format:
104  * There is one row per Gaussian "mode", and each row contains 10
105  * elements:
106  * - w (The linear weight)
107  * - x_mean (gaussian mean value)
108  * - y_mean (gaussian mean value)
109  * - x_mean (gaussian mean value)
110  * - yaw_mean (gaussian mean value, in radians)
111  * - pitch_mean (gaussian mean value, in radians)
112  * - roll_mean (gaussian mean value, in radians)
113  * - C11,C22,C33,C44,C55,C66 (Covariance elements)
114  * - C12,C13,C14,C15,C16 (Covariance elements)
115  * - C23,C24,C25,C25 (Covariance elements)
116  * - C34,C35,C36 (Covariance elements)
117  * - C45,C46 (Covariance elements)
118  * - C56 (Covariance elements)
119  *
120  */
121  bool saveToTextFile(const std::string& file) const override;
122 
123  /** this = p (+) this. This can be used to convert a PDF from local
124  * coordinates to global, providing the point (newReferenceBase) from which
125  * "to project" the current pdf. Result PDF substituted the currently
126  * stored one in the object. */
127  void changeCoordinatesReference(const CPose3D& newReferenceBase) override;
128 
129  /** Bayesian fusion of two pose distributions, then save the result in this
130  * object (WARNING: Currently p1 must be a mrpt::poses::CPose3DPDFSOG object
131  * and p2 a mrpt::poses::CPose3DPDFSOG object) */
132  void bayesianFusion(const CPose3DPDF& p1, const CPose3DPDF& p2) override;
133 
134  /** Draws a single sample from the distribution */
135  void drawSingleSample(CPose3D& outPart) const override;
136  /** Draws a number of samples from the distribution, and saves as a list of
137  * 1x6 vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum */
138  void drawManySamples(
139  size_t N,
140  std::vector<mrpt::math::CVectorDouble>& outSamples) const override;
141  /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF */
142  void inverse(CPose3DPDF& o) const override;
143 
144  /** Append the Gaussian modes from "o" to the current set of modes of "this"
145  * density */
146  void appendFrom(const CPose3DPDFSOG& o);
147 
148 }; // End of class def.
149 }
150 
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:32
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:65
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:51
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:80
void drawSingleSample(CPose3D &outPart) const override
Draws a single sample from the distribution.
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
TModesList::const_iterator const_iterator
Definition: CPose3DPDFSOG.h:52
#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:86
void clear()
Clear all the gaussian modes.
TModesList::iterator iterator
Definition: CPose3DPDFSOG.h:53
const_iterator end() const
Definition: CPose3DPDFSOG.h:84
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:78
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:83
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:40
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: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020