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 CPosePDFGaussian_H 00029 #define CPosePDFGaussian_H 00030 00031 #include <mrpt/poses/CPosePDF.h> 00032 #include <mrpt/math/CMatrixD.h> 00033 00034 namespace mrpt 00035 { 00036 namespace poses 00037 { 00038 using namespace mrpt::math; 00039 00040 class CPose3DPDF; 00041 00042 // This must be added to any CSerializable derived class: 00043 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFGaussian, CPosePDF ) 00044 00045 /** Declares a class that represents a Probability Density function (PDF) of a 2D pose \f$ p(\mathbf{x}) = [x ~ y ~ \phi ]^t \f$. 00046 * 00047 * This class implements that PDF using a mono-modal Gaussian distribution. See mrpt::poses::CPosePDF for more details. 00048 * 00049 * \sa CPose2D, CPosePDF, CPosePDFParticles 00050 */ 00051 class MRPTDLLIMPEXP CPosePDFGaussian : public CPosePDF 00052 { 00053 // This must be added to any CSerializable derived class: 00054 DEFINE_SERIALIZABLE( CPosePDFGaussian ) 00055 00056 protected: 00057 /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!) 00058 */ 00059 void assureSymmetry(); 00060 00061 public: 00062 /** Default constructor 00063 */ 00064 CPosePDFGaussian(); 00065 00066 /** Constructor 00067 */ 00068 CPosePDFGaussian( const CPose2D &init_Mean ); 00069 00070 /** Constructor 00071 */ 00072 CPosePDFGaussian( const CPose2D &init_Mean, const CMatrixDouble33 &init_Cov ); 00073 00074 /** Copy constructor, including transformations between other PDFs */ 00075 CPosePDFGaussian( const CPosePDF &o ) { copyFrom( o ); } 00076 00077 /** Copy constructor, including transformations between other PDFs */ 00078 CPosePDFGaussian( const CPose3DPDF &o ) { copyFrom( o ); } 00079 00080 /** The mean value 00081 */ 00082 CPose2D mean; 00083 00084 /** The 3x3 covariance matrix 00085 */ 00086 CMatrixDouble33 cov; 00087 00088 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). 00089 * \sa getCovariance 00090 */ 00091 void getMean(CPose2D &mean_pose) const; 00092 00093 /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once. 00094 * \sa getMean 00095 */ 00096 void getCovarianceAndMean(CMatrixDouble33 &cov,CPose2D &mean_point) const; 00097 00098 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00099 */ 00100 void copyFrom(const CPosePDF &o); 00101 00102 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00103 */ 00104 void copyFrom(const CPose3DPDF &o); 00105 00106 /** Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance matrix in next 3 lines. 00107 */ 00108 void saveToTextFile(const std::string &file) const; 00109 00110 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00111 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00112 */ 00113 void changeCoordinatesReference( const CPose3D &newReferenceBase ); 00114 00115 /** Rotate the covariance matrix by replacing it 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$. 00116 */ 00117 void rotateCov(const double& ang); 00118 00119 /** Set \f$ this = x1 \ominus x0 \f$ , computing the mean using the "-" operator and the covariances through the corresponding Jacobians (For 'x0' and 'x1' being independent variables!). 00120 */ 00121 void inverseComposition( const CPosePDFGaussian &x, const CPosePDFGaussian &ref ); 00122 00123 /** Set \f$ this = x1 \ominus x0 \f$ , computing the mean using the "-" operator and the covariances through the corresponding Jacobians (Given the 3x3 cross-covariance matrix of variables x0 and x1). 00124 */ 00125 void inverseComposition( 00126 const CPosePDFGaussian &x1, 00127 const CPosePDFGaussian &x0, 00128 const CMatrixDouble33 &COV_01 00129 ); 00130 00131 /** Draws a single sample from the distribution 00132 */ 00133 void drawSingleSample( CPose2D &outPart ) const; 00134 00135 /** 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. 00136 */ 00137 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00138 00139 /** Bayesian fusion of two points gauss. distributions, then save the result in this object. 00140 * The process is as follows:<br> 00141 * - (x1,S1): Mean and variance of the p1 distribution. 00142 * - (x2,S2): Mean and variance of the p2 distribution. 00143 * - (x,S): Mean and variance of the resulting distribution. 00144 * 00145 * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>; 00146 * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 ); 00147 */ 00148 void bayesianFusion( CPosePDF &p1, CPosePDF &p2, const double &minMahalanobisDistToDrop = 0 ); 00149 00150 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00151 */ 00152 void inverse(CPosePDF &o) const; 00153 00154 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). 00155 */ 00156 void operator += ( CPose2D Ap); 00157 00158 /** Evaluates the PDF at a given point. 00159 */ 00160 double evaluatePDF( const CPose2D &x ) const; 00161 00162 /** Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1]. 00163 */ 00164 double evaluateNormalizedPDF( const CPose2D &x ) const; 00165 00166 /** Computes the Mahalanobis distance between the centers of two Gaussians. 00167 */ 00168 double mahalanobisDistanceTo( const CPosePDFGaussian& theOther ); 00169 00170 /** Substitutes the diagonal elements if (square) they are below some given minimum values (Use this before bayesianFusion, for example, to avoid inversion of singular matrixes, etc...) 00171 */ 00172 void assureMinCovariance( const double & minStdXY, const double &minStdPhi ); 00173 00174 }; // End of class def. 00175 00176 00177 00178 /** Dumps the mean and covariance matrix to a text stream. 00179 */ 00180 std::ostream & operator << (std::ostream & out, const CPosePDFGaussian& obj); 00181 00182 /** Returns the Gaussian distribution of \f$ \mathbf{C} \f$, for \f$ \mathbf{C} = \mathbf{A} \oplus \mathbf{B} \f$. 00183 */ 00184 poses::CPosePDFGaussian operator + ( const mrpt::poses::CPose2D &A, const mrpt::poses::CPosePDFGaussian &B ); 00185 00186 bool MRPTDLLIMPEXP operator==(const CPosePDFGaussian &p1,const CPosePDFGaussian &p2); 00187 00188 } // End of namespace 00189 } // End of namespace 00190 00191 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:10:56 EDT 2009 |