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 CPosePDFSOG_H 00029 #define CPosePDFSOG_H 00030 00031 #include <mrpt/poses/CPosePDF.h> 00032 #include <mrpt/math/CMatrix.h> 00033 #include <mrpt/math/CMatrixD.h> 00034 00035 00036 namespace mrpt 00037 { 00038 namespace opengl { struct CSetOfObjectsPtr; } 00039 namespace poses 00040 { 00041 using namespace mrpt::math; 00042 00043 // This must be added to any CSerializable derived class: 00044 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFSOG , CPosePDF ) 00045 00046 /** Declares a class that represents a Probability Density function (PDF) of a 2D pose \f$ p(\mathbf{x}) = [x ~ y ~ \phi ]^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 CPosePDFSOG::m_modes 00052 * 00053 * See mrpt::poses::CPosePDF for more details. 00054 * 00055 * \sa CPose2D, CPosePDF, CPosePDFParticles 00056 */ 00057 class MRPTDLLIMPEXP CPosePDFSOG : public CPosePDF 00058 { 00059 // This must be added to any CSerializable derived class: 00060 DEFINE_SERIALIZABLE( CPosePDFSOG ) 00061 00062 public: 00063 /** The struct for each mode: 00064 */ 00065 struct MRPTDLLIMPEXP TGaussianMode 00066 { 00067 TGaussianMode() : 00068 mean(), 00069 cov(), 00070 log_w(0) 00071 { } 00072 00073 CPose2D mean; 00074 CMatrixDouble33 cov; 00075 00076 /** The log-weight 00077 */ 00078 double log_w; 00079 }; 00080 00081 typedef std::deque<TGaussianMode> CListGaussianModes; 00082 typedef std::deque<TGaussianMode>::const_iterator const_iterator; 00083 typedef std::deque<TGaussianMode>::iterator iterator; 00084 00085 protected: 00086 /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!) 00087 */ 00088 void assureSymmetry(); 00089 00090 /** The list of SOG modes */ 00091 CListGaussianModes m_modes; 00092 00093 public: 00094 /** Default constructor 00095 * \param nModes The initial size of CPosePDFSOG::m_modes 00096 */ 00097 CPosePDFSOG( size_t nModes = 1 ); 00098 00099 size_t size() const { return m_modes.size(); } //!< Return the number of Gaussian modes. 00100 bool empty() const { return m_modes.empty(); } //!< Return whether there is any Gaussian mode. 00101 00102 /** Clear the list of modes */ 00103 void clear(); 00104 00105 /** Access to individual beacons */ 00106 const TGaussianMode& operator [](size_t i) const { 00107 ASSERT_(i<m_modes.size()) 00108 return m_modes[i]; 00109 } 00110 /** Access to individual beacons */ 00111 TGaussianMode& operator [](size_t i) { 00112 ASSERT_(i<m_modes.size()) 00113 return m_modes[i]; 00114 } 00115 00116 /** Access to individual beacons */ 00117 const TGaussianMode& get(size_t i) const { 00118 ASSERT_(i<m_modes.size()) 00119 return m_modes[i]; 00120 } 00121 /** Access to individual beacons */ 00122 TGaussianMode& get(size_t i) { 00123 ASSERT_(i<m_modes.size()) 00124 return m_modes[i]; 00125 } 00126 00127 /** Inserts a copy of the given mode into the SOG */ 00128 void push_back(const TGaussianMode& m) { 00129 m_modes.push_back(m); 00130 } 00131 00132 iterator begin() { return m_modes.begin(); } 00133 iterator end() { return m_modes.end(); } 00134 const_iterator begin() const { return m_modes.begin(); } 00135 const_iterator end()const { return m_modes.end(); } 00136 00137 iterator erase(iterator i) { return m_modes.erase(i); } 00138 00139 void resize(const size_t N); //!< Resize the number of SOG modes 00140 00141 /** Merge very close modes so the overall number of modes is reduced while preserving the total distribution. 00142 * This method uses the approach described in the paper: 00143 * - "Kullback-Leibler Approach to Gaussian Mixture Reduction" AR Runnalls. IEEE Transactions on Aerospace and Electronic Systems, 2007. 00144 * 00145 * \param max_KLd The maximum KL-divergence to consider the merge of two nodes (and then stops the process). 00146 */ 00147 void mergeModes( double max_KLd = 0.5, bool verbose = false ); 00148 00149 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). 00150 * \sa getCovariance 00151 */ 00152 void getMean(CPose2D &mean_pose) const; 00153 00154 /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once. 00155 * \sa getMean 00156 */ 00157 void getCovarianceAndMean(CMatrixDouble33 &cov,CPose2D &mean_point) const; 00158 00159 /** For the most likely Gaussian mode in the SOG, returns the pose covariance matrix (3x3 cov matrix) and the mean. 00160 * \sa getMean 00161 */ 00162 void getMostLikelyCovarianceAndMean(CMatrixDouble33 &cov,CPose2D &mean_point) const; 00163 00164 /** Normalize the weights in m_modes such as the maximum log-weight is 0. 00165 */ 00166 void normalizeWeights(); 00167 00168 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00169 */ 00170 void copyFrom(const CPosePDF &o); 00171 00172 /** Save the density to a text file, with the following format: 00173 * There is one row per Gaussian "mode", and each row contains 10 elements: 00174 * - w (The weight) 00175 * - x_mean (gaussian mean value) 00176 * - y_mean (gaussian mean value) 00177 * - phi_mean (gaussian mean value) 00178 * - C11 (Covariance elements) 00179 * - C22 (Covariance elements) 00180 * - C33 (Covariance elements) 00181 * - C12 (Covariance elements) 00182 * - C13 (Covariance elements) 00183 * - C23 (Covariance elements) 00184 * 00185 */ 00186 void saveToTextFile(const std::string &file) const; 00187 00188 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00189 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00190 */ 00191 void changeCoordinatesReference(const CPose3D &newReferenceBase ); 00192 00193 /** Rotate all the covariance matrixes by replacing them by \f$ \mathbf{R}~\mathbf{COV}~\mathbf{R}^t \f$, where \f$ \mathbf{R} = \left[ \begin{array}{ccc} \cos\alpha & -\sin\alpha & 0 \\ \sin\alpha & \cos\alpha & 0 \\ 0 & 0 & 1 \end{array}\right] \f$. 00194 */ 00195 void rotateAllCovariances(const double &ang); 00196 00197 /** Draws a single sample from the distribution 00198 */ 00199 void drawSingleSample( CPose2D &outPart ) const; 00200 00201 /** 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. 00202 */ 00203 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00204 00205 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00206 */ 00207 void inverse(CPosePDF &o) const; 00208 00209 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). 00210 */ 00211 void operator += ( const CPose2D Ap); 00212 00213 /** Evaluates the PDF at a given point. 00214 */ 00215 double evaluatePDF( const CPose2D &x, bool sumOverAllPhis = false ) const; 00216 00217 /** Evaluates the ratio PDF(x) / max_PDF(x*), that is, the normalized PDF in the range [0,1]. 00218 */ 00219 double evaluateNormalizedPDF( const CPose2D &x ) const; 00220 00221 /** Evaluates the PDF within a rectangular grid (and a fixed orientation) and saves the result in a matrix (each row contains values for a fixed y-coordinate value). 00222 */ 00223 void evaluatePDFInArea( 00224 const double & x_min, 00225 const double & x_max, 00226 const double & y_min, 00227 const double & y_max, 00228 const double & resolutionXY, 00229 const double & phi, 00230 CMatrixD &outMatrix, 00231 bool sumOverAllPhis = false ); 00232 00233 /** Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1 must be a mrpt::poses::CPosePDFSOG object and p2 a mrpt::poses::CPosePDFGaussian object) 00234 */ 00235 void bayesianFusion( CPosePDF &p1, CPosePDF &p2, const double &minMahalanobisDistToDrop=0 ); 00236 00237 00238 /** Returns a 3D object representation of all the ellipses in the SoG. 00239 */ 00240 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const; 00241 00242 00243 }; // End of class def. 00244 00245 } // End of namespace 00246 } // End of namespace 00247 00248 #endif
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:21:34 EDT 2009 |