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 CPose3DPDFSOG_H 00029 #define CPose3DPDFSOG_H 00030 00031 #include <mrpt/poses/CPose3DPDF.h> 00032 #include <mrpt/poses/CPose3DPDFGaussian.h> 00033 #include <mrpt/math/CMatrix.h> 00034 00035 namespace mrpt 00036 { 00037 namespace poses 00038 { 00039 // This must be added to any CSerializable derived class: 00040 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3DPDFSOG, CPose3DPDF ) 00041 00042 /** Declares a class that represents a Probability Density function (PDF) of a 3D(6D) pose \f$ p(\mathbf{x}) = [x ~ y ~ z ~ yaw ~ pitch ~ roll]^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 CPose3DPDFSOG::m_modes. Angles are always in radians. 00048 * 00049 * See mrpt::poses::CPose3DPDF for more details. 00050 * 00051 * \sa CPose3DPDF 00052 */ 00053 class BASE_IMPEXP CPose3DPDFSOG : public CPose3DPDF 00054 { 00055 // This must be added to any CSerializable derived class: 00056 DEFINE_SERIALIZABLE( CPose3DPDFSOG ) 00057 00058 public: 00059 /** The struct for each mode: 00060 */ 00061 struct BASE_IMPEXP TGaussianMode 00062 { 00063 TGaussianMode() : 00064 val(), 00065 log_w(0) 00066 { } 00067 00068 CPose3DPDFGaussian val; 00069 00070 /** The log-weight 00071 */ 00072 double log_w; 00073 }; 00074 00075 typedef mrpt::aligned_containers<TGaussianMode>::vector_t TModesList; 00076 typedef TModesList::const_iterator const_iterator; 00077 typedef TModesList::iterator iterator; 00078 00079 protected: 00080 /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!) 00081 */ 00082 void assureSymmetry(); 00083 00084 /** Access directly to this array for modify the modes as desired. 00085 * Note that no weight can be zero!! 00086 * We must use pointers to satisfy the mem-alignment of the matrixes 00087 */ 00088 TModesList m_modes; 00089 00090 public: 00091 /** Default constructor 00092 * \param nModes The initial size of CPose3DPDFSOG::m_modes 00093 */ 00094 CPose3DPDFSOG( size_t nModes = 1 ); 00095 00096 void clear(); //!< Clear all the gaussian modes 00097 void resize(const size_t N); //!< Set the number of SOG modes 00098 size_t size() const { return m_modes.size(); } //!< Return the number of Gaussian modes. 00099 bool empty() const { return m_modes.empty(); } //!< Return whether there is any Gaussian mode. 00100 00101 iterator begin() { return m_modes.begin(); } 00102 iterator end() { return m_modes.end(); } 00103 const_iterator begin() const { return m_modes.begin(); } 00104 const_iterator end()const { return m_modes.end(); } 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 /** Normalize the weights in m_modes such as the maximum log-weight is 0. 00117 */ 00118 void normalizeWeights(); 00119 00120 /** Return the Gaussian mode with the highest likelihood (or an empty Gaussian if there are no modes in this SOG) */ 00121 void getMostLikelyMode( CPose3DPDFGaussian& outVal ) const; 00122 00123 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00124 */ 00125 void copyFrom(const CPose3DPDF &o); 00126 00127 /** Save the density to a text file, with the following format: 00128 * There is one row per Gaussian "mode", and each row contains 10 elements: 00129 * - w (The linear weight) 00130 * - x_mean (gaussian mean value) 00131 * - y_mean (gaussian mean value) 00132 * - x_mean (gaussian mean value) 00133 * - yaw_mean (gaussian mean value, in radians) 00134 * - pitch_mean (gaussian mean value, in radians) 00135 * - roll_mean (gaussian mean value, in radians) 00136 * - C11,C22,C33,C44,C55,C66 (Covariance elements) 00137 * - C12,C13,C14,C15,C16 (Covariance elements) 00138 * - C23,C24,C25,C25 (Covariance elements) 00139 * - C34,C35,C36 (Covariance elements) 00140 * - C45,C46 (Covariance elements) 00141 * - C56 (Covariance elements) 00142 * 00143 */ 00144 void saveToTextFile(const std::string &file) const; 00145 00146 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00147 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00148 */ 00149 void changeCoordinatesReference(const CPose3D &newReferenceBase ); 00150 00151 /** Bayesian fusion of two pose distributions, then save the result in this object (WARNING: Currently p1 must be a mrpt::poses::CPose3DPDFSOG object and p2 a mrpt::poses::CPose3DPDFSOG object) 00152 */ 00153 void bayesianFusion( const CPose3DPDF &p1,const CPose3DPDF &p2 ); 00154 00155 /** Draws a single sample from the distribution 00156 */ 00157 void drawSingleSample( CPose3D &outPart ) const; 00158 00159 /** Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,z,yaw,pitch,roll) datum. 00160 */ 00161 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00162 00163 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00164 */ 00165 void inverse(CPose3DPDF &o) const; 00166 00167 /** Append the Gaussian modes from "o" to the current set of modes of "this" density. 00168 */ 00169 void appendFrom( const CPose3DPDFSOG &o ); 00170 00171 }; // End of class def. 00172 00173 00174 } // End of namespace 00175 } // End of namespace 00176 00177 #endif
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:30:30 UTC 2011 |