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