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