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.1 for MRPT 0.9.4 SVN: at Mon Jan 10 23:33:19 UTC 2011 |