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 CPose3DPDFGaussian_H 00029 #define CPose3DPDFGaussian_H 00030 00031 #include <mrpt/poses/CPose3DPDF.h> 00032 #include <mrpt/poses/CPosePDF.h> 00033 #include <mrpt/math/CMatrixD.h> 00034 00035 namespace mrpt 00036 { 00037 namespace poses 00038 { 00039 class CPosePDFGaussian; 00040 00041 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3DPDFGaussian , CPose3DPDF ) 00042 00043 /** Declares a class that represents a Probability Density function (PDF) of a 3D pose \f$ p(\mathbf{x}) = [x ~ y ~ z ~ yaw ~ pitch ~ roll]^t \f$. 00044 * 00045 * This class implements that PDF using a mono-modal Gaussian distribution. See mrpt::poses::CPose3DPDF for more details. 00046 * 00047 * The formulas used for pose/uncertainty composition in the "CPose3DPDFGaussian::operator+=" and "CPose3DPDFGaussian::jacobiansPoseComposition": 00048 * 00049 * We compose the pose "x" by adding "u" to it, obtaining "y": 00050 * 00051 * \f[ y = x \oplus u \f] 00052 * 00053 * The mean of "y" is: 00054 * 00055 \f[ \hat{y} = \left( 00056 \begin{array}{l} 00057 \hat{x}_x + R_{11}\hat{u}_x + R_{12}\hat{u}_y + R_{13}\hat{u}_z \\ 00058 \hat{x}_y + R_{21}\hat{u}_x + R_{22}\hat{u}_y + R_{23}\hat{u}_z \\ 00059 \hat{x}_z + R_{31}\hat{u}_x + R_{32}\hat{u}_y + R_{33}\hat{u}_z \\ 00060 \hat{x}_{yaw} + \hat{u}_{yaw} \\ 00061 \hat{x}_p + \hat{u}_p \\ 00062 \hat{x}_r + \hat{u}_r 00063 \end{array} \right) 00064 \f] 00065 00066 * And its covariance: 00067 00068 \f[ 00069 \Sigma_y = \frac{df}{dx} \Sigma_x \frac{df}{dx}^\top + \frac{df}{du} \Sigma_u \frac{df}{du}^\top 00070 \f] 00071 00072 * With the Jacobians being computed by CPose3DPDFGaussian::jacobiansPoseComposition (see that method documentation for the exact formulas). 00073 * 00074 * \sa CPose3D, CPose3DPDF, CPose3DPDFParticles 00075 */ 00076 class MRPTDLLIMPEXP CPose3DPDFGaussian : public CPose3DPDF 00077 { 00078 // This must be added to any CSerializable derived class: 00079 DEFINE_SERIALIZABLE( CPose3DPDFGaussian ) 00080 00081 protected: 00082 /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!) 00083 */ 00084 void assureSymmetry(); 00085 00086 public: 00087 /** Default constructor 00088 */ 00089 CPose3DPDFGaussian(); 00090 00091 /** Constructor 00092 */ 00093 CPose3DPDFGaussian( const CPose3D &init_Mean ); 00094 00095 /** Constructor 00096 */ 00097 CPose3DPDFGaussian( const CPose3D &init_Mean, const CMatrixD &init_Cov ); 00098 00099 /** Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables z,pitch, and roll). 00100 */ 00101 CPose3DPDFGaussian( const CPosePDFGaussian &o ); 00102 00103 /** The mean value 00104 */ 00105 CPose3D mean; 00106 00107 /** The 6x6 covariance matrix 00108 */ 00109 CMatrixDouble66 cov; 00110 00111 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF), computed as a weighted average over all m_particles. 00112 * \sa getCovariance 00113 */ 00114 void getMean(CPose3D &mean_pose) const; 00115 00116 /** Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once. 00117 * \sa getMean 00118 */ 00119 void getCovarianceAndMean(CMatrixDouble66 &cov,CPose3D &mean_point) const; 00120 00121 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00122 */ 00123 void copyFrom(const CPose3DPDF &o); 00124 00125 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00126 */ 00127 void copyFrom(const CPosePDF &o); 00128 00129 /** Save PDF's particles to a text file, containing the 3D pose in the first line, then the covariance matrix in next 3 lines. 00130 */ 00131 void saveToTextFile(const std::string &file) const; 00132 00133 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00134 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00135 */ 00136 void changeCoordinatesReference( const CPose3D &newReferenceBase ); 00137 00138 /** Rotate the covariance matrix by replacing it by \f$ \mathbf{R}~\mathbf{COV}~\mathbf{R}^t \f$, where \f$ \mathbf{R} = ... \f$. 00139 */ 00140 void rotateCov(const double &ang); 00141 00142 /** Draws a single sample from the distribution 00143 */ 00144 void drawSingleSample( CPose3D &outPart ) const; 00145 00146 /** Draws a number of samples from the distribution, and saves as a list of 1x6 vectors, where each row contains a (x,y,phi) datum. 00147 */ 00148 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00149 00150 /** Bayesian fusion of two points gauss. distributions, then save the result in this object. 00151 * The process is as follows:<br> 00152 * - (x1,S1): Mean and variance of the p1 distribution. 00153 * - (x2,S2): Mean and variance of the p2 distribution. 00154 * - (x,S): Mean and variance of the resulting distribution. 00155 * 00156 * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>; 00157 * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 ); 00158 */ 00159 void bayesianFusion( CPose3DPDF &p1, CPose3DPDF &p2 ); 00160 00161 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00162 */ 00163 void inverse(CPose3DPDF &o) const; 00164 00165 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). 00166 */ 00167 void operator += ( const CPose3D &Ap); 00168 00169 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated) (see formulas on the top of this page). 00170 */ 00171 void operator += ( const CPose3DPDFGaussian &Ap); 00172 00173 /** Evaluates the PDF at a given point. 00174 */ 00175 double evaluatePDF( const CPose3D &x ) const; 00176 00177 /** Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1]. 00178 */ 00179 double evaluateNormalizedPDF( const CPose3D &x ) const; 00180 00181 /** Computes the Mahalanobis distance between the centers of two Gaussians. 00182 * The variables with a variance exactly equal to 0 are not taken into account in the process, but 00183 * "infinity" is returned if the corresponding elements are not exactly equal. 00184 */ 00185 double mahalanobisDistanceTo( const CPose3DPDFGaussian& theOther); 00186 00187 /** This static method computes the pose composition Jacobians, with these formulas: 00188 00189 \code 00190 df_dx = 00191 [ 1, 0, 0, -sin(yaw)*cos(p)*xu+(-sin(yaw)*sin(p)*sin(r)-cos(yaw)*cos(r))*yu+(-sin(yaw)*sin(p)*cos(r)+cos(yaw)*sin(r))*zu, -cos(yaw)*sin(p)*xu+cos(yaw)*cos(p)*sin(r)*yu+cos(yaw)*cos(p)*cos(r)*zu, (cos(yaw)*sin(p)*cos(r)+sin(yaw)*sin(r))*yu+(-cos(yaw)*sin(p)*sin(r)+sin(yaw)*cos(r))*zu] 00192 [ 0, 1, 0, cos(yaw)*cos(p)*xu+(cos(yaw)*sin(p)*sin(r)-sin(yaw)*cos(r))*yu+(cos(yaw)*sin(p)*cos(r)+sin(yaw)*sin(r))*zu, -sin(yaw)*sin(p)*xu+sin(yaw)*cos(p)*sin(r)*yu+sin(yaw)*cos(p)*cos(r)*zu, (sin(yaw)*sin(p)*cos(r)-cos(yaw)*sin(r))*yu+(-sin(yaw)*sin(p)*sin(r)-cos(yaw)*cos(r))*zu] 00193 [ 0, 0, 1, 0, -cos(p)*xu-sin(p)*sin(r)*yu-sin(p)*cos(r)*zu, cos(p)*cos(r)*yu-cos(p)*sin(r)*zu] 00194 [ 0, 0, 0, 1, 0, 0] 00195 [ 0, 0, 0, 0, 1, 0] 00196 [ 0, 0, 0, 0, 0, 1] 00197 00198 df_du = 00199 [ cos(yaw)*cos(p), cos(yaw)*sin(p)*sin(r)-sin(yaw)*cos(r), cos(yaw)*sin(p)*cos(r)+sin(yaw)*sin(r), 0, 0, 0] 00200 [ sin(yaw)*cos(p), sin(yaw)*sin(p)*sin(r)+cos(yaw)*cos(r), sin(yaw)*sin(p)*cos(r)-cos(yaw)*sin(r), 0, 0, 0] 00201 [ -sin(p), cos(p)*sin(r), cos(p)*cos(r), 0, 0, 0] 00202 [ 0, 0, 0, 1, 0, 0] 00203 [ 0, 0, 0, 0, 1, 0] 00204 [ 0, 0, 0, 0, 0, 1] 00205 \endcode 00206 */ 00207 static void jacobiansPoseComposition( 00208 const CPose3DPDFGaussian &x, 00209 const CPose3DPDFGaussian &u, 00210 CMatrixDouble66 &df_dx, 00211 CMatrixDouble66 &df_du); 00212 00213 00214 /** Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only. 00215 */ 00216 void getCovSubmatrix2D( CMatrixDouble &out_cov ) const; 00217 00218 00219 }; // End of class def. 00220 00221 00222 /** Pose composition for two 3D pose Gaussians (see formulas on the top of this page). 00223 * \sa CPose3DPDFGaussian::operator += 00224 */ 00225 CPose3DPDFGaussian MRPTDLLIMPEXP operator +( const CPose3DPDFGaussian &x, const CPose3DPDFGaussian &u ); 00226 00227 /** Dumps the mean and covariance matrix to a text stream. 00228 */ 00229 std::ostream MRPTDLLIMPEXP & operator << (std::ostream & out, const CPose3DPDFGaussian& obj); 00230 00231 bool MRPTDLLIMPEXP operator==(const CPose3DPDFGaussian &p1,const CPose3DPDFGaussian &p2); 00232 00233 } // End of namespace 00234 } // End of namespace 00235 00236 #endif
Page generated by Doxygen 1.5.7.1 for MRPT 0.7.1 SVN: at Mon Aug 17 23:10:56 EDT 2009 |