Main MRPT website > C++ reference
MRPT logo

CPointPDFSOG.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 CPointPDFSOG_H
00029 #define CPointPDFSOG_H
00030 
00031 #include <mrpt/poses/CPointPDF.h>
00032 #include <mrpt/poses/CPointPDFGaussian.h>
00033 #include <mrpt/math/CMatrix.h>
00034 #include <mrpt/math/CMatrixD.h>
00035 
00036 namespace mrpt
00037 {
00038         namespace poses
00039         {
00040                 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointPDFSOG, CPointPDF )
00041 
00042                 /** Declares a class that represents a Probability Density function (PDF) of a 3D point \f$ p(\mathbf{x}) = [x ~ y ~ z ]^t \f$.
00043                  *   This class implements that PDF as the following multi-modal Gaussian distribution:
00044                  *
00045                  * \f$ p(\mathbf{x}) = \sum\limits_{i=1}^N \omega^i \mathcal{N}( \mathbf{x} ; \bar{\mathbf{x}}^i, \mathbf{\Sigma}^i )  \f$
00046                  *
00047                  *  Where the number of modes N is the size of CPointPDFSOG::m_modes
00048                  *
00049                  *  See mrpt::poses::CPointPDF for more details.
00050                  *
00051                  * \sa CPointPDF, CPosePDF,
00052                  */
00053                 class BASE_IMPEXP  CPointPDFSOG : public CPointPDF
00054                 {
00055                         // This must be added to any CSerializable derived class:
00056                         DEFINE_SERIALIZABLE( CPointPDFSOG )
00057 
00058                 public:
00059                         /** The struct for each mode:
00060                          */
00061                         struct BASE_IMPEXP TGaussianMode
00062                         {
00063                                 TGaussianMode() : val(), log_w(0)
00064                                 {
00065                                 }
00066 
00067                                 CPointPDFGaussian               val;
00068 
00069                                 /** The log-weight
00070                                   */
00071                                 double          log_w;
00072                         };
00073 
00074                         typedef std::deque<TGaussianMode> CListGaussianModes;
00075                         typedef std::deque<TGaussianMode>::const_iterator const_iterator;
00076                         typedef std::deque<TGaussianMode>::iterator iterator;
00077 
00078                 protected:
00079                         /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!)
00080                           */
00081                         void  assureSymmetry();
00082 
00083                         /** The list of SOG modes */
00084                         CListGaussianModes      m_modes;
00085 
00086                  public:
00087                         /** Default constructor
00088                           * \param nModes The initial size of CPointPDFSOG::m_modes
00089                           */
00090                         CPointPDFSOG( size_t nModes = 1 );
00091 
00092                         void clear(); //!< Clear all the gaussian modes
00093 
00094                         /** Access to individual beacons */
00095                         const TGaussianMode& operator [](size_t i) const {
00096                                 ASSERT_(i<m_modes.size())
00097                                 return  m_modes[i];
00098                         }
00099                         /** Access to individual beacons */
00100                         TGaussianMode& operator [](size_t i) {
00101                                 ASSERT_(i<m_modes.size())
00102                                 return  m_modes[i];
00103                         }
00104 
00105                         /** Access to individual beacons */
00106                         const TGaussianMode& get(size_t i) const {
00107                                 ASSERT_(i<m_modes.size())
00108                                 return  m_modes[i];
00109                         }
00110                         /** Access to individual beacons */
00111                         TGaussianMode& get(size_t i) {
00112                                 ASSERT_(i<m_modes.size())
00113                                 return  m_modes[i];
00114                         }
00115 
00116                         /** Inserts a copy of the given mode into the SOG */
00117                         void push_back(const TGaussianMode& m) {
00118                                 m_modes.push_back(m);
00119                         }
00120 
00121                         iterator begin() { return m_modes.begin(); }
00122                         iterator end() { return m_modes.end(); }
00123                         const_iterator begin() const { return m_modes.begin(); }
00124                         const_iterator end()const { return m_modes.end(); }
00125                         
00126                         iterator erase(iterator i) { return m_modes.erase(i); }
00127 
00128                         void resize(const size_t N); //!< Resize the number of SOG modes 
00129                         size_t size() const { return m_modes.size(); } //!< Return the number of Gaussian modes.
00130                         bool empty() const { return m_modes.empty(); } //!< Return whether there is any Gaussian mode.
00131 
00132                          /** Returns an estimate of the point, (the mean, or mathematical expectation of the PDF).
00133                            * \sa getCovariance
00134                            */
00135                         void getMean(CPoint3D &mean_point) const;
00136 
00137                         /** Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once.
00138                           * \sa getMean
00139                           */
00140                         void getCovarianceAndMean(CMatrixDouble33 &cov,CPoint3D &mean_point) const;
00141 
00142                         /** Normalize the weights in m_modes such as the maximum log-weight is 0.
00143                           */
00144                         void  normalizeWeights();
00145 
00146                         /** Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG) */
00147                         void getMostLikelyMode( CPointPDFGaussian& outVal ) const;
00148 
00149                         /** Computes the "Effective sample size" (typical measure for Particle Filters), applied to the weights of the individual Gaussian modes, as a measure of the equality of the modes (in the range [0,total # of modes]).
00150                           */
00151                         double ESS() const;
00152 
00153                         /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
00154                           */
00155                         void  copyFrom(const CPointPDF &o);
00156 
00157                         /** Save the density to a text file, with the following format:
00158                           *  There is one row per Gaussian "mode", and each row contains 10 elements:
00159                           *   - w (The weight)
00160                           *   - x_mean (gaussian mean value)
00161                           *   - y_mean (gaussian mean value)
00162                           *   - x_mean (gaussian mean value)
00163                           *   - C11 (Covariance elements)
00164                           *   - C22 (Covariance elements)
00165                           *   - C33 (Covariance elements)
00166                           *   - C12 (Covariance elements)
00167                           *   - C13 (Covariance elements)
00168                           *   - C23 (Covariance elements)
00169                           *
00170                          */
00171                         void  saveToTextFile(const std::string &file) const;
00172 
00173                         /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
00174                           *   "to project" the current pdf. Result PDF substituted the currently stored one in the object.
00175                           */
00176                         void  changeCoordinatesReference(const CPose3D &newReferenceBase );
00177 
00178                         /** Draw a sample from the pdf.
00179                           */
00180                         void drawSingleSample(CPoint3D  &outSample) const;
00181 
00182                         /** Bayesian fusion of two point distributions (product of two distributions->new distribution), then save the result in this object (WARNING: See implementing classes to see classes that can and cannot be mixtured!)
00183                           * \param p1 The first distribution to fuse
00184                           * \param p2 The second distribution to fuse
00185                           * \param minMahalanobisDistToDrop If set to different of 0, the result of very separate Gaussian modes (that will result in negligible components) in SOGs will be dropped to reduce the number of modes in the output.
00186                           */
00187                         void  bayesianFusion( const CPointPDF &p1, const CPointPDF &p2, const double &minMahalanobisDistToDrop = 0);
00188 
00189 
00190                         /** Evaluates the PDF within a rectangular grid and saves the result in a matrix (each row contains values for a fixed y-coordinate value).
00191                           */
00192                         void  evaluatePDFInArea(
00193                                 float           x_min,
00194                                 float           x_max,
00195                                 float           y_min,
00196                                 float           y_max,
00197                                 float           resolutionXY,
00198                                 float           z,
00199                                 CMatrixD        &outMatrix,
00200                                 bool            sumOverAllZs = false );
00201 
00202                         /** Evaluates the PDF at a given point.
00203                           */
00204                         double  evaluatePDF(
00205                                 const   CPoint3D &x,
00206                                 bool    sumOverAllZs ) const;
00207 
00208 
00209                 }; // End of class def.
00210 
00211 
00212         } // End of namespace
00213 } // End of namespace
00214 
00215 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011