00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 CPosePDFGaussianInf_H 00029 #define CPosePDFGaussianInf_H 00030 00031 #include <mrpt/poses/CPosePDF.h> 00032 #include <mrpt/math/CMatrixFixedNumeric.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( CPosePDFGaussianInf, CPosePDF ) 00044 00045 /** A Probability Density function (PDF) of a 2D pose \f$ p(\mathbf{x}) = [x ~ y ~ \phi ]^t \f$ as a Gaussian with a mean and the inverse of the covariance. 00046 * 00047 * This class implements a PDF as a mono-modal Gaussian distribution in its <b>information form</b>, that is, 00048 * keeping the inverse of the covariance matrix instead of the covariance matrix itself. 00049 * 00050 * This class is the dual of CPosePDFGaussian. 00051 * 00052 * \sa CPose2D, CPosePDF, CPosePDFParticles 00053 * \ingroup poses_pdf_grp 00054 */ 00055 class BASE_IMPEXP CPosePDFGaussianInf : public CPosePDF 00056 { 00057 // This must be added to any CSerializable derived class: 00058 DEFINE_SERIALIZABLE( CPosePDFGaussianInf ) 00059 00060 protected: 00061 /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!) 00062 */ 00063 void assureSymmetry(); 00064 00065 public: 00066 /** @name Data fields 00067 @{ */ 00068 00069 CPose2D mean; //!< The mean value 00070 CMatrixDouble33 cov_inv; //!< The inverse of the 3x3 covariance matrix (the "information" matrix) 00071 00072 /** @} */ 00073 00074 inline const CPose2D & getPoseMean() const { return mean; } 00075 inline CPose2D & getPoseMean() { return mean; } 00076 00077 /** Default constructor (mean=all zeros, inverse covariance=all zeros -> so be careful!) */ 00078 CPosePDFGaussianInf(); 00079 00080 /** Constructor with a mean value (inverse covariance=all zeros -> so be careful!) */ 00081 explicit CPosePDFGaussianInf( const CPose2D &init_Mean ); 00082 00083 /** Constructor */ 00084 CPosePDFGaussianInf( const CPose2D &init_Mean, const CMatrixDouble33 &init_CovInv ); 00085 00086 /** Copy constructor, including transformations between other PDFs */ 00087 explicit CPosePDFGaussianInf( const CPosePDF &o ) { copyFrom( o ); } 00088 00089 /** Copy constructor, including transformations between other PDFs */ 00090 explicit CPosePDFGaussianInf( const CPose3DPDF &o ) { copyFrom( o ); } 00091 00092 00093 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). 00094 * \sa getCovariance 00095 */ 00096 void getMean(CPose2D &mean_pose) const; 00097 00098 /** Returns an estimate of the pose covariance matrix (3x3 cov matrix) and the mean, both at once. 00099 * \sa getMean 00100 */ 00101 void getCovarianceAndMean(CMatrixDouble33 &cov,CPose2D &mean_point) const; 00102 00103 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00104 */ 00105 void copyFrom(const CPosePDF &o); 00106 00107 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00108 */ 00109 void copyFrom(const CPose3DPDF &o); 00110 00111 /** Save PDF's particles to a text file, containing the 2D pose in the first line, then the covariance matrix in next 3 lines. 00112 */ 00113 void saveToTextFile(const std::string &file) const; 00114 00115 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00116 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00117 */ 00118 void changeCoordinatesReference( const CPose3D &newReferenceBase ); 00119 00120 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00121 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00122 */ 00123 void changeCoordinatesReference( const CPose2D &newReferenceBase ); 00124 00125 /** 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$. 00126 */ 00127 void rotateCov(const double ang); 00128 00129 /** 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!). 00130 */ 00131 void inverseComposition( const CPosePDFGaussianInf &x, const CPosePDFGaussianInf &ref ); 00132 00133 /** 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). 00134 */ 00135 void inverseComposition( 00136 const CPosePDFGaussianInf &x1, 00137 const CPosePDFGaussianInf &x0, 00138 const CMatrixDouble33 &COV_01 00139 ); 00140 00141 /** Draws a single sample from the distribution 00142 */ 00143 void drawSingleSample( CPose2D &outPart ) const; 00144 00145 /** 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. 00146 */ 00147 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00148 00149 /** Bayesian fusion of two points gauss. distributions, then save the result in this object. 00150 * The process is as follows:<br> 00151 * - (x1,S1): Mean and variance of the p1 distribution. 00152 * - (x2,S2): Mean and variance of the p2 distribution. 00153 * - (x,S): Mean and variance of the resulting distribution. 00154 * 00155 * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>; 00156 * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 ); 00157 */ 00158 void bayesianFusion(const CPosePDF &p1,const CPosePDF &p2, const double &minMahalanobisDistToDrop = 0 ); 00159 00160 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00161 */ 00162 void inverse(CPosePDF &o) const; 00163 00164 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). */ 00165 void operator += ( const CPose2D &Ap); 00166 00167 /** Evaluates the PDF at a given point. 00168 */ 00169 double evaluatePDF( const CPose2D &x ) const; 00170 00171 /** Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1]. 00172 */ 00173 double evaluateNormalizedPDF( const CPose2D &x ) const; 00174 00175 /** Computes the Mahalanobis distance between the centers of two Gaussians. 00176 */ 00177 double mahalanobisDistanceTo( const CPosePDFGaussianInf& theOther ); 00178 00179 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated) (see formulas in jacobiansPoseComposition ). 00180 */ 00181 void operator += ( const CPosePDFGaussianInf &Ap); 00182 00183 /** Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated) 00184 */ 00185 inline void operator -=( const CPosePDFGaussianInf &ref ) { 00186 this->inverseComposition(*this,ref); 00187 } 00188 00189 00190 00191 /** This static method computes the pose composition Jacobians, with these formulas: 00192 \code 00193 df_dx = 00194 [ 1, 0, -sin(phi_x)*x_u-cos(phi_x)*y_u ] 00195 [ 0, 1, cos(phi_x)*x_u-sin(phi_x)*y_u ] 00196 [ 0, 0, 1 ] 00197 00198 df_du = 00199 [ cos(phi_x) , -sin(phi_x) , 0 ] 00200 [ sin(phi_x) , cos(phi_x) , 0 ] 00201 [ 0 , 0 , 1 ] 00202 \endcode 00203 */ 00204 static void jacobiansPoseComposition( 00205 const CPosePDFGaussianInf &x, 00206 const CPosePDFGaussianInf &u, 00207 CMatrixDouble33 &df_dx, 00208 CMatrixDouble33 &df_du); 00209 00210 00211 00212 }; // End of class def. 00213 00214 00215 /** Pose compose operator: RES = A (+) B , computing both the mean and the covariance */ 00216 inline CPosePDFGaussianInf operator +( const CPosePDFGaussianInf &a, const CPosePDFGaussianInf &b ) { 00217 CPosePDFGaussianInf res(a); 00218 res+=b; 00219 return res; 00220 } 00221 00222 /** Pose inverse compose operator: RES = A (-) B , computing both the mean and the covariance */ 00223 inline CPosePDFGaussianInf operator -( const CPosePDFGaussianInf &a, const CPosePDFGaussianInf &b ) { 00224 CPosePDFGaussianInf res; 00225 res.inverseComposition(a,b); 00226 return res; 00227 } 00228 00229 /** Dumps the mean and covariance matrix to a text stream. 00230 */ 00231 std::ostream BASE_IMPEXP & operator << (std::ostream & out, const CPosePDFGaussianInf& obj); 00232 00233 /** Returns the Gaussian distribution of \f$ \mathbf{C} \f$, for \f$ \mathbf{C} = \mathbf{A} \oplus \mathbf{B} \f$. 00234 */ 00235 poses::CPosePDFGaussianInf BASE_IMPEXP operator + ( const mrpt::poses::CPose2D &A, const mrpt::poses::CPosePDFGaussianInf &B ); 00236 00237 bool BASE_IMPEXP operator==(const CPosePDFGaussianInf &p1,const CPosePDFGaussianInf &p2); 00238 00239 } // End of namespace 00240 } // End of namespace 00241 00242 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |