Main MRPT website > C++ reference
MRPT logo

CPoseRandomSampler.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef CPoseRandomSampler_H
00029 #define CPoseRandomSampler_H
00030 
00031 #include <mrpt/poses/CPosePDF.h>
00032 #include <mrpt/poses/CPose3DPDF.h>
00033 #include <mrpt/math/CMatrixTemplateNumeric.h>
00034 
00035 namespace mrpt
00036 {
00037     namespace poses
00038     {
00039         using namespace mrpt::math;
00040 
00041         /** An efficient generator of random samples drawn from a given 2D (CPosePDF) or 3D (CPose3DPDF) pose probability density function (pdf).
00042          * This class keeps an internal state which speeds up the sequential generation of samples. It can manage
00043          *  any kind of pose PDF.
00044                  *
00045          * Use with CPoseRandomSampler::setPosePDF, then CPoseRandomSampler::drawSample to draw values.
00046                  *
00047          * Notice that you can pass a 2D or 3D pose PDF, then ask for a 2D or 3D sample. This class always returns
00048          *  the kind of sample you ask it for, but will skip missing terms or fill out with zeroes as required.
00049                  * Specifically, when sampling 3D poses from a 2D pose pdf, this class will be smart enought to draw only 
00050                  *  the 3 required dimensions, avoiding a waste of time with the other 3 missing components.
00051          *
00052          * \sa CPosePDF, CPose3DPDF
00053          */
00054         class BASE_IMPEXP CPoseRandomSampler
00055         {
00056         protected:
00057                         // Only ONE of these can be not-NULL at a time.
00058             CPosePDF*   m_pdf2D;        //!< A local copy of the PDF
00059             CPose3DPDF* m_pdf3D;        //!< A local copy of the PDF
00060 
00061             CMatrixDouble33 m_fastdraw_gauss_Z3;
00062             CMatrixDouble66 m_fastdraw_gauss_Z6;
00063             CPose2D         m_fastdraw_gauss_M_2D;
00064             CPose3D         m_fastdraw_gauss_M_3D;
00065 
00066             void clear(); //!< Clear internal pdf
00067 
00068                         void do_sample_2D( CPose2D &p ) const;  //!< Used internally: sample from m_pdf2D
00069                         void do_sample_3D( CPose3D &p ) const;  //!< Used internally: sample from m_pdf3D
00070 
00071         public:
00072             /** Default constructor */
00073             CPoseRandomSampler();
00074 
00075             /** Destructor */
00076             ~CPoseRandomSampler();
00077 
00078             /** This method must be called to select the PDF from which to draw samples.
00079               * \sa drawSample
00080               */
00081             void setPosePDF( const CPosePDF *pdf );
00082 
00083             /** This method must be called to select the PDF from which to draw samples.
00084               * \sa drawSample
00085               */
00086             void setPosePDF( const CPosePDFPtr &pdf ) { setPosePDF(pdf.pointer()); }
00087 
00088             /** This method must be called to select the PDF from which to draw samples.
00089               * \sa drawSample
00090               */
00091             void setPosePDF( const CPosePDF &pdf ) { setPosePDF(&pdf); }
00092 
00093             /** This method must be called to select the PDF from which to draw samples.
00094               * \sa drawSample
00095               */
00096             void setPosePDF( const CPose3DPDF *pdf );
00097 
00098             /** This method must be called to select the PDF from which to draw samples.
00099               * \sa drawSample
00100               */
00101             void setPosePDF( const CPose3DPDFPtr &pdf ) { setPosePDF(pdf.pointer()); }
00102 
00103             /** This method must be called to select the PDF from which to draw samples.
00104               * \sa drawSample
00105               */
00106             void setPosePDF( const CPose3DPDF &pdf ) { setPosePDF(&pdf); }
00107 
00108             /** Generate a new sample from the selected PDF.
00109               * \return A reference to the same object passed as argument.
00110               * \sa setPosePDF
00111               */
00112             CPose2D & drawSample( CPose2D &p ) const;
00113 
00114             /** Generate a new sample from the selected PDF.
00115               * \return A reference to the same object passed as argument.
00116               * \sa setPosePDF
00117               */
00118             CPose3D & drawSample( CPose3D &p ) const;
00119 
00120                         /** Return true if samples can be generated, which only requires a previous call to setPosePDF */
00121                         bool isPrepared() const;
00122 
00123                         /** 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 */
00124                         CPose2D &getSamplingMean2D(CPose2D &out_mean) const;
00125 
00126                         /** 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 */
00127                         CPose3D &getSamplingMean3D(CPose3D &out_mean) const;
00128 
00129                         /** Retrieves the 3x3 covariance of the original PDF in \f$ [ x ~ y ~ \phi ] \f$. */
00130                         void getOriginalPDFCov2D( CMatrixDouble33 &cov3x3 ) const;
00131 
00132                         /** Retrieves the 3x3 covariance of the original PDF in \f$ [ x ~ y ~ \phi ] \f$. */
00133                         inline void getOriginalPDFCov2D( CMatrixDouble &cov3x3 ) const {
00134                                 CMatrixDouble33 M;
00135                                 this->getOriginalPDFCov2D(M);
00136                                 cov3x3 = CMatrixDouble(M);
00137                         }
00138 
00139                         /** Retrieves the 6x6 covariance of the original PDF in \f$ [ x ~ y ~ z ~ yaw ~ pitch ~ roll ] \f$. */
00140                         void getOriginalPDFCov3D( CMatrixDouble66 &cov6x6 ) const;
00141 
00142                         /** Retrieves the 6x6 covariance of the original PDF in \f$ [ x ~ y ~ z ~ yaw ~ pitch ~ roll ] \f$. */
00143                         inline void getOriginalPDFCov3D( CMatrixDouble &cov6x6 ) const {
00144                                 CMatrixDouble66 M;
00145                                 this->getOriginalPDFCov3D(M);
00146                                 cov6x6 = CMatrixDouble(M);
00147                         }
00148 
00149         }; // End of class def.
00150         } // End of namespace
00151 } // End of namespace
00152 
00153 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011