Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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  */
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  void getMean(CPose3D &mean_pose) const MRPT_OVERRIDE; //!< Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles. \sa getCovariance
88  void getCovarianceAndMean(mrpt::math::CMatrixDouble66 &cov,CPose3D &mean_point) const MRPT_OVERRIDE; //!< Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once. \sa getMean
89  void normalizeWeights(); //!< Normalize the weights in m_modes such as the maximum log-weight is 0.
90  void getMostLikelyMode( CPose3DPDFGaussian& outVal ) const; //!< Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG)
91 
92  void copyFrom(const CPose3DPDF &o) MRPT_OVERRIDE; //!< Copy operator, translating if necesary (for example, between particles and gaussian representations)
93 
94  /** Save the density to a text file, with the following format:
95  * There is one row per Gaussian "mode", and each row contains 10 elements:
96  * - w (The linear weight)
97  * - x_mean (gaussian mean value)
98  * - y_mean (gaussian mean value)
99  * - x_mean (gaussian mean value)
100  * - yaw_mean (gaussian mean value, in radians)
101  * - pitch_mean (gaussian mean value, in radians)
102  * - roll_mean (gaussian mean value, in radians)
103  * - C11,C22,C33,C44,C55,C66 (Covariance elements)
104  * - C12,C13,C14,C15,C16 (Covariance elements)
105  * - C23,C24,C25,C25 (Covariance elements)
106  * - C34,C35,C36 (Covariance elements)
107  * - C45,C46 (Covariance elements)
108  * - C56 (Covariance elements)
109  *
110  */
111  void saveToTextFile(const std::string &file) const MRPT_OVERRIDE;
112 
113  /** this = p (+) this. This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
114  * "to project" the current pdf. Result PDF substituted the currently stored one in the object. */
115  void changeCoordinatesReference(const CPose3D &newReferenceBase ) MRPT_OVERRIDE;
116 
117  /** 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) */
118  void bayesianFusion( const CPose3DPDF &p1,const CPose3DPDF &p2 ) MRPT_OVERRIDE;
119 
120  void drawSingleSample( CPose3D &outPart ) const MRPT_OVERRIDE; //!< Draws a single sample from the distribution
121  void drawManySamples( size_t N, std::vector<mrpt::math::CVectorDouble> & outSamples ) const MRPT_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
122  void inverse(CPose3DPDF &o) const MRPT_OVERRIDE; //!< Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
123 
124  void appendFrom( const CPose3DPDFSOG &o ); //!< Append the Gaussian modes from "o" to the current set of modes of "this" density
125 
126  }; // End of class def.
127  DEFINE_SERIALIZABLE_POST_CUSTOM_BASE( CPose3DPDFSOG, CPose3DPDF )
128  } // End of namespace
129 } // End of namespace
130 #endif
DEFINE_SERIALIZABLE_POST_CUSTOM_BASE(CPose3DPDFGaussianInf, CPose3DPDF)
TModesList::iterator iterator
Definition: CPose3DPDFSOG.h:58
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
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
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...
const Scalar * const_iterator
Definition: eigen_plugins.h:24
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:113
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
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
int val
Definition: mrpt_jpeglib.h:953
bool empty() const
Return whether there is any Gaussian mode.
Definition: CPose3DPDFSOG.h:80
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(class_name, base_name)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
GLsizei const GLchar ** string
Definition: glext.h:3919
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
const_iterator end() const
Definition: CPose3DPDFSOG.h:85
Declares a class that represents a Probability Density function (PDF) of a 3D pose ...
size_t size() const
Return the number of Gaussian modes.
Definition: CPose3DPDFSOG.h:79
const_iterator begin() const
Definition: CPose3DPDFSOG.h:84
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.14 for MRPT 1.5.7 Git: 8277875f6 Mon Jun 11 02:47:32 2018 +0200 at lun oct 28 01:50:49 CET 2019