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 CPointPDFParticles_H 00029 #define CPointPDFParticles_H 00030 00031 #include <mrpt/poses/CPointPDF.h> 00032 #include <mrpt/math/CMatrix.h> 00033 #include <mrpt/bayes/CProbabilityParticle.h> 00034 #include <mrpt/bayes/CParticleFilterData.h> 00035 00036 namespace mrpt 00037 { 00038 namespace poses 00039 { 00040 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( TSimple3DPoint, mrpt::utils::CSerializable ) 00041 00042 /** Data within each particle 00043 */ 00044 class BASE_IMPEXP TSimple3DPoint : public mrpt::utils::CSerializable 00045 { 00046 // This must be added to any CSerializable derived class: 00047 DEFINE_SERIALIZABLE( TSimple3DPoint ) 00048 public: 00049 TSimple3DPoint(const TSimple3DPoint&o) : x(o.x),y(o.y),z(o.z) 00050 { 00051 } 00052 00053 TSimple3DPoint() : x(0),y(0),z(0) 00054 { 00055 } 00056 00057 TSimple3DPoint(const CPoint3D &v) : x(v.x()),y(v.y()),z(v.z()) 00058 { 00059 } 00060 00061 float x,y,z; 00062 }; 00063 00064 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointPDFParticles, CPointPDF ) 00065 00066 /** A probability distribution of a 2D/3D point, represented as a set of random samples (particles). 00067 * \sa CPointPDF 00068 */ 00069 class BASE_IMPEXP CPointPDFParticles : public CPointPDF, public mrpt::bayes::CParticleFilterData<TSimple3DPoint> 00070 { 00071 // This must be added to any CSerializable derived class: 00072 DEFINE_SERIALIZABLE( CPointPDFParticles ) 00073 00074 // This uses CParticleFilterData to implement some methods required for CParticleFilterCapable: 00075 IMPLEMENT_PARTICLE_FILTER_CAPABLE(TSimple3DPoint); 00076 00077 public: 00078 /** Default constructor 00079 */ 00080 CPointPDFParticles(size_t numParticles = 1); 00081 00082 /** Destructor 00083 */ 00084 virtual ~CPointPDFParticles(); 00085 00086 /** Clear all the particles (free memory) 00087 */ 00088 void clear() { setSize(0); } 00089 00090 /** Erase all the previous particles and change the number of particles, with a given initial value 00091 */ 00092 void setSize(size_t numberParticles, const CPoint3D &defaultValue = CPoint3D(0,0,0) ); 00093 00094 /** Returns the number of particles 00095 */ 00096 size_t size() const 00097 { 00098 return m_particles.size(); 00099 } 00100 00101 /** Returns an estimate of the point, (the mean, or mathematical expectation of the PDF). 00102 * \sa getCovariance 00103 */ 00104 void getMean(CPoint3D &mean_point) const; 00105 00106 /** Returns an estimate of the point covariance matrix (3x3 cov matrix) and the mean, both at once. 00107 * \sa getMean 00108 */ 00109 void getCovarianceAndMean(CMatrixDouble33 &cov,CPoint3D &mean_point) const; 00110 00111 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00112 */ 00113 void copyFrom(const CPointPDF &o); 00114 00115 /** Save PDF's particles to a text file, where each line is: X Y Z LOG_W 00116 */ 00117 void saveToTextFile(const std::string &file) const; 00118 00119 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00120 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. Both the mean value and the covariance matrix are updated correctly. 00121 */ 00122 void changeCoordinatesReference( const CPose3D &newReferenceBase ); 00123 00124 /** Compute the kurtosis of the distribution. 00125 */ 00126 double computeKurtosis(); 00127 00128 /** Draw a sample from the pdf. 00129 */ 00130 void drawSingleSample(CPoint3D &outSample) const; 00131 00132 /** 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!) 00133 * \param p1 The first distribution to fuse 00134 * \param p2 The second distribution to fuse 00135 * \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. 00136 */ 00137 void bayesianFusion( const CPointPDF &p1, const CPointPDF &p2, const double &minMahalanobisDistToDrop = 0); 00138 00139 }; // End of class def. 00140 00141 00142 } // End of namespace 00143 } // End of namespace 00144 00145 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011 |