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 CPose3DPDFGaussianInf_H 00029 #define CPose3DPDFGaussianInf_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 class CPose3DQuatPDFGaussian; 00041 00042 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3DPDFGaussianInf , CPose3DPDF ) 00043 00044 /** 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$ as a Gaussian described by its mean and its inverse covariance matrix. 00045 * 00046 * This class implements that PDF using a mono-modal Gaussian distribution in "information" form (inverse covariance matrix). 00047 * 00048 * Uncertainty of pose composition operations (\f$ y = x \oplus u \f$) is implemented in the method "CPose3DPDFGaussianInf::operator+=". 00049 * 00050 * For further details on implemented methods and the theory behind them, 00051 * see <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a>. 00052 * 00053 * \sa CPose3D, CPose3DPDF, CPose3DPDFParticles, CPose3DPDFGaussian 00054 */ 00055 class BASE_IMPEXP CPose3DPDFGaussianInf : public CPose3DPDF 00056 { 00057 // This must be added to any CSerializable derived class: 00058 DEFINE_SERIALIZABLE( CPose3DPDFGaussianInf ) 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 CPose3D mean; //!< The mean value 00070 CMatrixDouble66 cov_inv; //!< The inverse of the 6x6 covariance matrix 00071 00072 /** @} */ 00073 00074 00075 /** Default constructor - mean: all zeros, inverse covariance=all zeros -> so be careful! 00076 */ 00077 CPose3DPDFGaussianInf(); 00078 00079 /** Constructor with a mean value, inverse covariance=all zeros -> so be careful! */ 00080 explicit CPose3DPDFGaussianInf( const CPose3D &init_Mean ); 00081 00082 /** Uninitialized constructor: leave all fields uninitialized - Call with UNINITIALIZED_POSE as argument 00083 */ 00084 CPose3DPDFGaussianInf(TConstructorFlags_Poses constructor_dummy_param); 00085 00086 /** Constructor with mean and inv cov. */ 00087 CPose3DPDFGaussianInf( const CPose3D &init_Mean, const CMatrixDouble66 &init_CovInv ); 00088 00089 /** Constructor from a 6D pose PDF described as a Quaternion 00090 */ 00091 explicit CPose3DPDFGaussianInf( const CPose3DQuatPDFGaussian &o); 00092 00093 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). 00094 * \sa getCovariance 00095 */ 00096 void getMean(CPose3D &mean_pose) const; 00097 00098 /** Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once. 00099 * \sa getMean 00100 */ 00101 void getCovarianceAndMean(CMatrixDouble66 &cov,CPose3D &mean_point) const; 00102 00103 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00104 */ 00105 void copyFrom(const CPose3DPDF &o); 00106 00107 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00108 */ 00109 void copyFrom(const CPosePDF &o); 00110 00111 /** Copy from a 6D pose PDF described as a Quaternion 00112 */ 00113 void copyFrom( const CPose3DQuatPDFGaussian &o); 00114 00115 /** Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in next 3 lines. 00116 */ 00117 void saveToTextFile(const std::string &file) const; 00118 00119 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00120 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00121 */ 00122 void changeCoordinatesReference( const CPose3D &newReferenceBase ); 00123 00124 /** Draws a single sample from the distribution 00125 */ 00126 void drawSingleSample( CPose3D &outPart ) const; 00127 00128 /** 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. 00129 */ 00130 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00131 00132 /** Bayesian fusion of two points gauss. distributions, then save the result in this object. 00133 * The process is as follows:<br> 00134 * - (x1,S1): Mean and variance of the p1 distribution. 00135 * - (x2,S2): Mean and variance of the p2 distribution. 00136 * - (x,S): Mean and variance of the resulting distribution. 00137 * 00138 * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>; 00139 * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 ); 00140 */ 00141 void bayesianFusion( const CPose3DPDF &p1, const CPose3DPDF &p2 ); 00142 00143 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00144 */ 00145 void inverse(CPose3DPDF &o) const; 00146 00147 /** Unary - operator, returns the PDF of the inverse pose. */ 00148 inline CPose3DPDFGaussianInf operator -() const 00149 { 00150 CPose3DPDFGaussianInf p(UNINITIALIZED_POSE); 00151 this->inverse(p); 00152 return p; 00153 } 00154 00155 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). 00156 */ 00157 void operator += ( const CPose3D &Ap); 00158 00159 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). 00160 */ 00161 void operator += ( const CPose3DPDFGaussianInf &Ap); 00162 00163 /** Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated). 00164 */ 00165 void operator -= ( const CPose3DPDFGaussianInf &Ap); 00166 00167 /** Evaluates the PDF at a given point. 00168 */ 00169 double evaluatePDF( const CPose3D &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 CPose3D &x ) const; 00174 00175 /** Computes the Mahalanobis distance between the centers of two Gaussians. 00176 * The variables with a variance exactly equal to 0 are not taken into account in the process, but 00177 * "infinity" is returned if the corresponding elements are not exactly equal. 00178 */ 00179 double mahalanobisDistanceTo( const CPose3DPDFGaussianInf& theOther); 00180 00181 /** This static method computes the pose composition Jacobians, with these formulas: 00182 \code 00183 df_dx = 00184 [ 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] 00185 [ 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] 00186 [ 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] 00187 [ 0, 0, 0, 1, 0, 0] 00188 [ 0, 0, 0, 0, 1, 0] 00189 [ 0, 0, 0, 0, 0, 1] 00190 00191 df_du = 00192 [ 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] 00193 [ 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] 00194 [ -sin(p), cos(p)*sin(r), cos(p)*cos(r), 0, 0, 0] 00195 [ 0, 0, 0, 1, 0, 0] 00196 [ 0, 0, 0, 0, 1, 0] 00197 [ 0, 0, 0, 0, 0, 1] 00198 \endcode 00199 */ 00200 static void jacobiansPoseComposition( 00201 const CPose3D &x, 00202 const CPose3D &u, 00203 CMatrixDouble66 &df_dx, 00204 CMatrixDouble66 &df_du); 00205 00206 /** Returns a 3x3 matrix with submatrix of the inverse covariance for the variables (x,y,yaw) only. 00207 */ 00208 void getInvCovSubmatrix2D( CMatrixDouble &out_cov ) const; 00209 00210 }; // End of class def. 00211 00212 00213 /** Pose composition for two 3D pose Gaussians \sa CPose3DPDFGaussian::operator += */ 00214 inline CPose3DPDFGaussianInf operator +( const CPose3DPDFGaussianInf &x, const CPose3DPDFGaussianInf &u ) 00215 { 00216 CPose3DPDFGaussianInf res(x); 00217 res+=u; 00218 return res; 00219 } 00220 00221 /** Pose composition for two 3D pose Gaussians \sa CPose3DPDFGaussianInf::operator -= */ 00222 inline CPose3DPDFGaussianInf operator -( const CPose3DPDFGaussianInf &x, const CPose3DPDFGaussianInf &u ) 00223 { 00224 CPose3DPDFGaussianInf res(x); 00225 res-=u; 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 CPose3DPDFGaussianInf& obj); 00232 00233 bool BASE_IMPEXP operator==(const CPose3DPDFGaussianInf &p1,const CPose3DPDFGaussianInf &p2); 00234 00235 } // End of namespace 00236 } // End of namespace 00237 00238 #endif
Page generated by Doxygen 1.7.2 for MRPT 0.9.4 SVN: at Mon Jan 10 22:46:17 UTC 2011 |