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



Page generated by Doxygen 1.8.9.1 for MRPT 1.3.2 SVN:Unversioned directory at Tue Dec 8 09:49:21 UTC 2015