00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 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 CPose3DPDFParticles_H 00029 #define CPose3DPDFParticles_H 00030 00031 #include <mrpt/poses/CPose3DPDF.h> 00032 #include <mrpt/slam/CMultiMetricMap.h> 00033 #include <mrpt/bayes/CProbabilityParticle.h> 00034 #include <mrpt/bayes/CParticleFilterCapable.h> 00035 #include <mrpt/bayes/CParticleFilterData.h> 00036 00037 namespace mrpt 00038 { 00039 namespace slam 00040 { 00041 class COccupancyGridMap2D; 00042 class CSensoryFrame; 00043 } 00044 00045 namespace poses 00046 { 00047 using namespace mrpt::bayes; 00048 using namespace mrpt::utils; 00049 using namespace mrpt::slam; 00050 00051 /** \typedef CProbabilityParticle<CPose3D> CPose3DParticle 00052 * A type definition for m_particles containing a 3D pose. 00053 */ 00054 typedef CProbabilityParticle<CPose3D> CPose3DParticle; 00055 00056 // This must be added to any CSerializable derived class: 00057 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE(CPose3DPDFParticles,CPose3DPDF) 00058 00059 /** Declares a class that represents a Probability Density function (PDF) of a 3D pose 00060 * This class implements that PDF using a set of m_particles 00061 * (for using particle filter methods), where M weighted 00062 * m_particles represent the PDF. 00063 * 00064 * \sa CPose3D, CPose3DPDF, CPoseGaussianPDF 00065 */ 00066 class MRPTDLLIMPEXP CPose3DPDFParticles : public CPose3DPDF, public mrpt::bayes::CParticleFilterData<CPose3D> 00067 { 00068 // This must be added to any CSerializable derived class: 00069 DEFINE_SERIALIZABLE( CPose3DPDFParticles ) 00070 00071 // This uses CParticleFilterData to implement some methods required for CParticleFilterCapable: 00072 IMPLEMENT_PARTICLE_FILTER_CAPABLE(CPose3D) 00073 00074 public: 00075 /** Constructor 00076 * \param M The number of m_particles. 00077 */ 00078 CPose3DPDFParticles( size_t M = 1 ); 00079 00080 /** Copy constructor: 00081 */ 00082 CPose3DPDFParticles( const CPose3DPDFParticles& obj ) : 00083 CPose3DPDF(), 00084 CParticleFilterData<CPose3D>() 00085 { 00086 copyFrom( obj ); 00087 } 00088 00089 /** Destructor 00090 */ 00091 virtual ~CPose3DPDFParticles(); 00092 00093 00094 /** Copy operator, translating if necesary (for example, between m_particles and gaussian representations) 00095 */ 00096 void copyFrom(const CPose3DPDF &o); 00097 00098 /** Reset the PDF to a single point: All m_particles will be set exactly to the supplied pose. 00099 * \param location The location to set all the m_particles. 00100 * \param particlesCount If this is set to 0 the number of m_particles remains unchanged. 00101 * \sa resetUniform, resetUniformFreeSpace 00102 */ 00103 void resetDeterministic( const CPose3D &location, 00104 size_t particlesCount = 0); 00105 00106 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles. 00107 * \sa getCovariance 00108 */ 00109 void getMean(CPose3D &mean_pose) const; 00110 00111 /** Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once. 00112 * \sa getMean 00113 */ 00114 void getCovarianceAndMean(CMatrixDouble66 &cov,CPose3D &mean_point) const; 00115 00116 /** Returns the pose of the i'th particle. 00117 */ 00118 CPose3D getParticlePose(int i) const; 00119 00120 /** Save PDF's m_particles to a text file. In each line it will go: "x y z" 00121 */ 00122 void saveToTextFile(const std::string &file) const; 00123 00124 /** Get the m_particles count (equivalent to "particlesCount") 00125 */ 00126 size_t size() const { return m_particles.size(); } 00127 00128 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00129 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00130 */ 00131 void changeCoordinatesReference( const CPose3D &newReferenceBase ); 00132 00133 /** Draws a single sample from the distribution (WARNING: weights are assumed to be normalized!) 00134 */ 00135 void drawSingleSample( CPose3D &outPart ) const; 00136 00137 /** Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum. 00138 */ 00139 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00140 00141 /** Appends (pose-composition) a given pose "p" to each particle 00142 */ 00143 void operator += ( const CPose3D &Ap); 00144 00145 /** Appends (add to the list) a set of m_particles to the existing ones, and then normalize weights. 00146 */ 00147 void append( CPose3DPDFParticles &o ); 00148 00149 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00150 */ 00151 void inverse(CPose3DPDF &o) const; 00152 00153 /** Returns the particle with the highest weight. 00154 */ 00155 CPose3D getMostLikelyParticle() const; 00156 00157 /** Bayesian fusion. 00158 */ 00159 void bayesianFusion( CPose3DPDF &p1, CPose3DPDF &p2 ); 00160 00161 }; // End of class def. 00162 00163 00164 } // End of namespace 00165 } // End of namespace 00166 00167 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:10:56 EDT 2009 |