Main MRPT website > C++ reference
MRPT logo
CPoseRandomSampler.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CPoseRandomSampler_H
29 #define CPoseRandomSampler_H
30 
31 #include <mrpt/poses/CPosePDF.h>
32 #include <mrpt/poses/CPose3DPDF.h>
33 #include <mrpt/math/CMatrixTemplateNumeric.h>
34 
35 namespace mrpt
36 {
37  namespace poses
38  {
39  using namespace mrpt::math;
40 
41  /** An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose probability density function (pdf).
42  * This class keeps an internal state which speeds up the sequential generation of samples. It can manage
43  * any kind of pose PDF.
44  *
45  * Use with CPoseRandomSampler::setPosePDF, then CPoseRandomSampler::drawSample to draw values.
46  *
47  * Notice that you can pass a 2D or 3D pose PDF, then ask for a 2D or 3D sample. This class always returns
48  * the kind of sample you ask it for, but will skip missing terms or fill out with zeroes as required.
49  * Specifically, when sampling 3D poses from a 2D pose pdf, this class will be smart enought to draw only
50  * the 3 required dimensions, avoiding a waste of time with the other 3 missing components.
51  *
52  * \ingroup poses_pdf_grp
53  * \sa CPosePDF, CPose3DPDF
54  */
55  class BASE_IMPEXP CPoseRandomSampler
56  {
57  protected:
58  // Only ONE of these can be not-NULL at a time.
59  CPosePDF* m_pdf2D; //!< A local copy of the PDF
60  CPose3DPDF* m_pdf3D; //!< A local copy of the PDF
61 
62  CMatrixDouble33 m_fastdraw_gauss_Z3;
63  CMatrixDouble66 m_fastdraw_gauss_Z6;
64  CPose2D m_fastdraw_gauss_M_2D;
65  CPose3D m_fastdraw_gauss_M_3D;
66 
67  void clear(); //!< Clear internal pdf
68 
69  void do_sample_2D( CPose2D &p ) const; //!< Used internally: sample from m_pdf2D
70  void do_sample_3D( CPose3D &p ) const; //!< Used internally: sample from m_pdf3D
71 
72  public:
73  /** Default constructor */
74  CPoseRandomSampler();
75 
76  /** Destructor */
77  ~CPoseRandomSampler();
78 
79  /** This method must be called to select the PDF from which to draw samples.
80  * \sa drawSample
81  */
82  void setPosePDF( const CPosePDF *pdf );
83 
84  /** This method must be called to select the PDF from which to draw samples.
85  * \sa drawSample
86  */
87  void setPosePDF( const CPosePDFPtr &pdf ) { setPosePDF(pdf.pointer()); }
88 
89  /** This method must be called to select the PDF from which to draw samples.
90  * \sa drawSample
91  */
92  void setPosePDF( const CPosePDF &pdf ) { setPosePDF(&pdf); }
93 
94  /** This method must be called to select the PDF from which to draw samples.
95  * \sa drawSample
96  */
97  void setPosePDF( const CPose3DPDF *pdf );
98 
99  /** This method must be called to select the PDF from which to draw samples.
100  * \sa drawSample
101  */
102  void setPosePDF( const CPose3DPDFPtr &pdf ) { setPosePDF(pdf.pointer()); }
103 
104  /** This method must be called to select the PDF from which to draw samples.
105  * \sa drawSample
106  */
107  void setPosePDF( const CPose3DPDF &pdf ) { setPosePDF(&pdf); }
108 
109  /** Generate a new sample from the selected PDF.
110  * \return A reference to the same object passed as argument.
111  * \sa setPosePDF
112  */
113  CPose2D & drawSample( CPose2D &p ) const;
114 
115  /** Generate a new sample from the selected PDF.
116  * \return A reference to the same object passed as argument.
117  * \sa setPosePDF
118  */
119  CPose3D & drawSample( CPose3D &p ) const;
120 
121  /** Return true if samples can be generated, which only requires a previous call to setPosePDF */
122  bool isPrepared() const;
123 
124  /** If the object has been loaded with setPosePDF this method returns the 2D pose mean samples will be drawn around. \return A reference to the argument */
125  CPose2D &getSamplingMean2D(CPose2D &out_mean) const;
126 
127  /** If the object has been loaded with setPosePDF this method returns the 3D pose mean samples will be drawn around. \return A reference to the argument */
128  CPose3D &getSamplingMean3D(CPose3D &out_mean) const;
129 
130  /** Retrieves the 3x3 covariance of the original PDF in \f$ [ x ~ y ~ \phi ] \f$. */
131  void getOriginalPDFCov2D( CMatrixDouble33 &cov3x3 ) const;
132 
133  /** Retrieves the 3x3 covariance of the original PDF in \f$ [ x ~ y ~ \phi ] \f$. */
134  inline void getOriginalPDFCov2D( CMatrixDouble &cov3x3 ) const {
135  CMatrixDouble33 M;
136  this->getOriginalPDFCov2D(M);
137  cov3x3 = CMatrixDouble(M);
138  }
139 
140  /** Retrieves the 6x6 covariance of the original PDF in \f$ [ x ~ y ~ z ~ yaw ~ pitch ~ roll ] \f$. */
141  void getOriginalPDFCov3D( CMatrixDouble66 &cov6x6 ) const;
142 
143  /** Retrieves the 6x6 covariance of the original PDF in \f$ [ x ~ y ~ z ~ yaw ~ pitch ~ roll ] \f$. */
144  inline void getOriginalPDFCov3D( CMatrixDouble &cov6x6 ) const {
145  CMatrixDouble66 M;
146  this->getOriginalPDFCov3D(M);
147  cov6x6 = CMatrixDouble(M);
148  }
149 
150  }; // End of class def.
151  } // End of namespace
152 } // End of namespace
153 
154 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013