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 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 class CPose3DQuatPDFGaussian; 00041 00042 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3DPDFGaussian , 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$. 00045 * 00046 * This class implements that PDF using a mono-modal Gaussian distribution. See mrpt::poses::CPose3DPDF for more details. 00047 * 00048 * Uncertainty of pose composition operations (\f$ y = x \oplus u \f$) is implemented in the method "CPose3DPDFGaussian::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 00054 */ 00055 class BASE_IMPEXP CPose3DPDFGaussian : public CPose3DPDF 00056 { 00057 // This must be added to any CSerializable derived class: 00058 DEFINE_SERIALIZABLE( CPose3DPDFGaussian ) 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 /** Default constructor 00067 */ 00068 CPose3DPDFGaussian(); 00069 00070 /** Constructor 00071 */ 00072 explicit CPose3DPDFGaussian( const CPose3D &init_Mean ); 00073 00074 /** Uninitialized constructor: leave all fields uninitialized - Call with UNINITIALIZED_POSE as argument 00075 */ 00076 CPose3DPDFGaussian(TConstructorFlags_Poses constructor_dummy_param); 00077 00078 MRPT_DECLARE_DEPRECATED_FUNCTION("Deprecated: use the constructor accepting a CMatrixDouble66 instead.", 00079 /** *DEPRECATED* Constructor */ 00080 CPose3DPDFGaussian( const CPose3D &init_Mean, const CMatrixD &init_Cov ) 00081 ); 00082 00083 /** Constructor */ 00084 CPose3DPDFGaussian( const CPose3D &init_Mean, const CMatrixDouble66 &init_Cov ); 00085 00086 /** Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables z,pitch, and roll). 00087 */ 00088 explicit CPose3DPDFGaussian( const CPosePDFGaussian &o ); 00089 00090 /** Constructor from a 6D pose PDF described as a Quaternion 00091 */ 00092 explicit CPose3DPDFGaussian( const CPose3DQuatPDFGaussian &o); 00093 00094 /** The mean value 00095 */ 00096 CPose3D mean; 00097 00098 /** The 6x6 covariance matrix 00099 */ 00100 CMatrixDouble66 cov; 00101 00102 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). 00103 * \sa getCovariance 00104 */ 00105 void getMean(CPose3D &mean_pose) const; 00106 00107 /** Returns an estimate of the pose covariance matrix (6x6 cov matrix) and the mean, both at once. 00108 * \sa getMean 00109 */ 00110 void getCovarianceAndMean(CMatrixDouble66 &cov,CPose3D &mean_point) const; 00111 00112 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00113 */ 00114 void copyFrom(const CPose3DPDF &o); 00115 00116 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00117 */ 00118 void copyFrom(const CPosePDF &o); 00119 00120 /** Copy from a 6D pose PDF described as a Quaternion 00121 */ 00122 void copyFrom( const CPose3DQuatPDFGaussian &o); 00123 00124 00125 /** Save the PDF to a text file, containing the 3D pose in the first line, then the covariance matrix in next 3 lines. 00126 */ 00127 void saveToTextFile(const std::string &file) const; 00128 00129 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00130 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00131 */ 00132 void changeCoordinatesReference( const CPose3D &newReferenceBase ); 00133 00134 /** Draws a single sample from the distribution 00135 */ 00136 void drawSingleSample( CPose3D &outPart ) const; 00137 00138 /** 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. 00139 */ 00140 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00141 00142 /** Bayesian fusion of two points gauss. distributions, then save the result in this object. 00143 * The process is as follows:<br> 00144 * - (x1,S1): Mean and variance of the p1 distribution. 00145 * - (x2,S2): Mean and variance of the p2 distribution. 00146 * - (x,S): Mean and variance of the resulting distribution. 00147 * 00148 * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>; 00149 * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 ); 00150 */ 00151 void bayesianFusion( const CPose3DPDF &p1, const CPose3DPDF &p2 ); 00152 00153 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00154 */ 00155 void inverse(CPose3DPDF &o) const; 00156 00157 /** Unary - operator, returns the PDF of the inverse pose. */ 00158 inline CPose3DPDFGaussian operator -() const 00159 { 00160 CPose3DPDFGaussian p(UNINITIALIZED_POSE); 00161 this->inverse(p); 00162 return p; 00163 } 00164 00165 00166 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). 00167 */ 00168 void operator += ( const CPose3D &Ap); 00169 00170 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). 00171 */ 00172 void operator += ( const CPose3DPDFGaussian &Ap); 00173 00174 /** Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated). 00175 */ 00176 void operator -= ( const CPose3DPDFGaussian &Ap); 00177 00178 /** Evaluates the PDF at a given point. 00179 */ 00180 double evaluatePDF( const CPose3D &x ) const; 00181 00182 /** Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1]. 00183 */ 00184 double evaluateNormalizedPDF( const CPose3D &x ) const; 00185 00186 /** Computes the Mahalanobis distance between the centers of two Gaussians. 00187 * The variables with a variance exactly equal to 0 are not taken into account in the process, but 00188 * "infinity" is returned if the corresponding elements are not exactly equal. 00189 */ 00190 double mahalanobisDistanceTo( const CPose3DPDFGaussian& theOther); 00191 00192 /** This static method computes the pose composition Jacobians. 00193 * 00194 * See this techical report: http:///www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty 00195 * 00196 * Direct equations (for the covariances) in yaw-pitch-roll are too complex. 00197 * Make a way around them and consider instead this path: 00198 * \code 00199 * X(6D) U(6D) 00200 * | | 00201 * v v 00202 * X(7D) U(7D) 00203 * | | 00204 * +--- (+) ---+ 00205 * | 00206 * v 00207 * RES(7D) 00208 * | 00209 * v 00210 * RES(6D) 00211 * \endcode 00212 * 00213 */ 00214 static void jacobiansPoseComposition( 00215 const CPose3D &x, 00216 const CPose3D &u, 00217 CMatrixDouble66 &df_dx, 00218 CMatrixDouble66 &df_du); 00219 00220 00221 /** Returns a 3x3 matrix with submatrix of the covariance for the variables (x,y,yaw) only. 00222 */ 00223 void getCovSubmatrix2D( CMatrixDouble &out_cov ) const; 00224 00225 00226 }; // End of class def. 00227 00228 00229 /** Pose composition for two 3D pose Gaussians \sa CPose3DPDFGaussian::operator += */ 00230 inline CPose3DPDFGaussian operator +( const CPose3DPDFGaussian &x, const CPose3DPDFGaussian &u ) 00231 { 00232 CPose3DPDFGaussian res(x); 00233 res+=u; 00234 return res; 00235 } 00236 00237 /** Pose composition for two 3D pose Gaussians \sa CPose3DPDFGaussian::operator -= */ 00238 inline CPose3DPDFGaussian operator -( const CPose3DPDFGaussian &x, const CPose3DPDFGaussian &u ) 00239 { 00240 CPose3DPDFGaussian res(x); 00241 res-=u; 00242 return res; 00243 } 00244 00245 /** Dumps the mean and covariance matrix to a text stream. 00246 */ 00247 std::ostream BASE_IMPEXP & operator << (std::ostream & out, const CPose3DPDFGaussian& obj); 00248 00249 bool BASE_IMPEXP operator==(const CPose3DPDFGaussian &p1,const CPose3DPDFGaussian &p2); 00250 00251 } // End of namespace 00252 00253 00254 /** Global variables to change the run-time behaviour of some MRPT classes within mrpt-core. 00255 * See each variable for the description of what classes it affects. 00256 */ 00257 namespace global_settings 00258 { 00259 /** If set to true (false), a Scaled Unscented Transform is used instead of a linear approximation with Jacobians. 00260 * Affects to: 00261 * - CPose3DPDFGaussian::CPose3DPDFGaussian( const CPose3DQuatPDFGaussian &o) 00262 */ 00263 extern BASE_IMPEXP bool USE_SUT_QUAT2EULER_CONVERSION; 00264 } 00265 00266 } // End of namespace 00267 00268 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011 |