Main MRPT website > C++ reference
MRPT logo

CPosePDFGrid.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 CPosePDFGrid_H
00029 #define CPosePDFGrid_H
00030 
00031 #include <mrpt/poses/CPosePDF.h>
00032 #include <mrpt/poses/CPose2DGridTemplate.h>
00033 
00034 namespace mrpt
00035 {
00036 namespace poses
00037 {
00038         using namespace mrpt::utils;
00039 
00040         // This must be added to any CSerializable derived class:
00041         DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFGrid, CPosePDF   )
00042 
00043         /** Declares a class that represents a Probability Distribution
00044          *    function (PDF) of a 2D pose (x,y,phi).
00045          *   This class implements that PDF using a 3D grid.
00046      *
00047          * \sa CPose2D, CPosePDF, CPose2DGridTemplate
00048          */
00049         class BASE_IMPEXP CPosePDFGrid : public CPosePDF, public CPose2DGridTemplate<double>
00050         {
00051                 // This must be added to any CSerializable derived class:
00052                 DEFINE_SERIALIZABLE( CPosePDFGrid )
00053 
00054          protected:
00055 
00056 
00057          public:
00058                 /** Constructor: Initializes a, uniform distribution over the whole given range.
00059                   */
00060                 CPosePDFGrid(
00061                         double          xMin = -1.0f,
00062                         double          xMax = 1.0f,
00063                         double          yMin = -1.0f,
00064                         double          yMax = 1.0f,
00065                         double          resolutionXY = 0.5f,
00066                         double          resolutionPhi = DEG2RAD(180),
00067                         double          phiMin = -M_PIf,
00068                         double          phiMax = M_PIf
00069                         );
00070 
00071                 /** Destructor
00072                  */
00073                 virtual ~CPosePDFGrid();
00074 
00075                 /** Copy operator, translating if necesary (for example, between particles and gaussian representations)
00076                   */
00077                 void  copyFrom(const CPosePDF &o);
00078 
00079                 /** Normalizes the PDF, such as all cells sum the unity.
00080                   */
00081                 void  normalize();
00082 
00083                 /** Assigns the same value to all the cells in the grid, so the sum 1.
00084                   */
00085                 void  uniformDistribution();
00086 
00087                  /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF).
00088                    * \sa getCovariance
00089                    */
00090                 void getMean(CPose2D &mean_pose) const;
00091 
00092                 /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once.
00093                   * \sa getMean
00094                   */
00095                 void getCovarianceAndMean(CMatrixDouble33 &cov,CPose2D &mean_point) const;
00096 
00097                 /** Save the contents of the 3D grid in one file, as a vertical concatenation of rectangular matrix for the different "PHI" discrete levels, and the size in X,Y,and PHI in another file named "<filename>_dims.txt"
00098                  */
00099                 void   saveToTextFile(const std::string &dataFile) const;
00100 
00101                 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which
00102                   *   "to project" the current pdf. Result PDF substituted the currently stored one in the object.
00103                   */
00104                 void  changeCoordinatesReference( const CPose3D &newReferenceBase );
00105 
00106                 /** Bayesian fusion of 2 densities (In the grid representation this becomes a pointwise multiplication)
00107                   */
00108                 void  bayesianFusion(const  CPosePDF &p1,const  CPosePDF &p2, const double &minMahalanobisDistToDrop = 0 );
00109 
00110                 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF
00111                   */
00112                 void  inverse(CPosePDF &o) const;
00113 
00114                 /** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!)
00115                   */
00116                 void  drawSingleSample( CPose2D &outPart ) const;
00117 
00118                 /** Draws a number of samples from the distribution, and saves as a list of 1x3 vectors, where each row contains a (x,y,phi) datum.
00119                   */
00120                 void  drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const;
00121 
00122         }; // End of class def.
00123 
00124 
00125         } // End of namespace
00126 } // End of namespace
00127 
00128 #endif



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