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 CPose3DQuatPDFGaussian_H 00029 #define CPose3DQuatPDFGaussian_H 00030 00031 #include <mrpt/poses/CPose3DQuatPDF.h> 00032 #include <mrpt/poses/CPose3DPDF.h> 00033 #include <mrpt/poses/CPosePDF.h> 00034 #include <mrpt/math/CMatrixD.h> 00035 00036 namespace mrpt 00037 { 00038 namespace poses 00039 { 00040 class CPosePDFGaussian; 00041 class CPose3DPDFGaussian; 00042 00043 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3DQuatPDFGaussian , CPose3DQuatPDF ) 00044 00045 /** Declares a class that represents a Probability Density function (PDF) of a 3D pose using a quaternion \f$ p(\mathbf{x}) = [x ~ y ~ z ~ qr ~ qx ~ qy ~ qz]^\top \f$. 00046 * 00047 * This class implements that PDF using a mono-modal Gaussian distribution. See mrpt::poses::CPose3DQuatPDF for more details, or 00048 * mrpt::poses::CPose3DPDF for classes based on Euler angles instead. 00049 * 00050 * Uncertainty of pose composition operations (\f$ y = x \oplus u \f$) is implemented in the methods "CPose3DQuatPDFGaussian::operator+=" and "CPose3DQuatPDFGaussian::jacobiansPoseComposition". 00051 * 00052 * For further details on implemented methods and the theory behind them, 00053 * see <a href="http://www.mrpt.org/6D_poses:equivalences_compositions_and_uncertainty" >this report</a>. 00054 * 00055 * \sa CPose3DQuat, CPose3DQuatPDF, CPose3DPDF 00056 * \ingroup poses_pdf_grp 00057 */ 00058 class BASE_IMPEXP CPose3DQuatPDFGaussian : public CPose3DQuatPDF 00059 { 00060 // This must be added to any CSerializable derived class: 00061 DEFINE_SERIALIZABLE( CPose3DQuatPDFGaussian ) 00062 00063 protected: 00064 /** Assures the symmetry of the covariance matrix (eventually certain operations in the math-coprocessor lead to non-symmetric matrixes!) 00065 */ 00066 void assureSymmetry(); 00067 00068 public: 00069 /** Default constructor - set all values to zero. */ 00070 CPose3DQuatPDFGaussian(); 00071 00072 /** Constructor which left all the member uninitialized, for using when speed is critical - as argument, use UNINITIALIZED_QUATERNION. */ 00073 CPose3DQuatPDFGaussian(TConstructorFlags_Quaternions constructor_dummy_param); 00074 00075 /** Constructor from a default mean value, covariance equals to zero. */ 00076 explicit CPose3DQuatPDFGaussian( const CPose3DQuat &init_Mean ); 00077 00078 /** Constructor with mean and covariance. */ 00079 CPose3DQuatPDFGaussian( const CPose3DQuat &init_Mean, const CMatrixDouble77 &init_Cov ); 00080 00081 /** Constructor from a Gaussian 2D pose PDF (sets to 0 the missing variables). */ 00082 explicit CPose3DQuatPDFGaussian( const CPosePDFGaussian &o ); 00083 00084 /** Constructor from an equivalent Gaussian in Euler angles representation. */ 00085 explicit CPose3DQuatPDFGaussian( const CPose3DPDFGaussian &o ); 00086 00087 /** The mean value */ 00088 CPose3DQuat mean; 00089 00090 /** The 7x7 covariance matrix */ 00091 CMatrixDouble77 cov; 00092 00093 inline const CPose3DQuat & getPoseMean() const { return mean; } 00094 inline CPose3DQuat & getPoseMean() { return mean; } 00095 00096 /** Returns an estimate of the pose, (the mean, or mathematical expectation of the PDF). 00097 * \sa getCovariance 00098 */ 00099 void getMean(CPose3DQuat &mean_pose) const; 00100 00101 /** Returns an estimate of the pose covariance matrix (7x7 cov matrix) and the mean, both at once. 00102 * \sa getMean 00103 */ 00104 void getCovarianceAndMean(CMatrixDouble77 &cov,CPose3DQuat &mean_point) const; 00105 00106 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00107 */ 00108 void copyFrom(const CPose3DQuatPDF &o); 00109 00110 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00111 */ 00112 void copyFrom(const CPosePDF &o); 00113 00114 /** Copy operator, translating if necesary (for example, between particles and gaussian representations) 00115 */ 00116 void copyFrom(const CPose3DPDFGaussian &o); 00117 00118 /** Save the PDF to a text file, containing the 3D pose in the first line (x y z qr qx qy qz), then the covariance matrix in the next 7 lines. 00119 */ 00120 void saveToTextFile(const std::string &file) const; 00121 00122 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00123 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00124 */ 00125 void changeCoordinatesReference( const CPose3DQuat &newReferenceBase ); 00126 00127 /** This can be used to convert a PDF from local coordinates to global, providing the point (newReferenceBase) from which 00128 * "to project" the current pdf. Result PDF substituted the currently stored one in the object. 00129 */ 00130 void changeCoordinatesReference( const CPose3D &newReferenceBase ); 00131 00132 /** Draws a single sample from the distribution 00133 */ 00134 void drawSingleSample( CPose3DQuat &outPart ) const; 00135 00136 /** Draws a number of samples from the distribution, and saves as a list of 1x7 vectors, where each row contains a (x,y,z,qr,qx,qy,qz) datum. 00137 */ 00138 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const; 00139 00140 /** Bayesian fusion of two points gauss. distributions, then save the result in this object. 00141 * The process is as follows:<br> 00142 * - (x1,S1): Mean and variance of the p1 distribution. 00143 * - (x2,S2): Mean and variance of the p2 distribution. 00144 * - (x,S): Mean and variance of the resulting distribution. 00145 * 00146 * S = (S1<sup>-1</sup> + S2<sup>-1</sup>)<sup>-1</sup>; 00147 * x = S * ( S1<sup>-1</sup>*x1 + S2<sup>-1</sup>*x2 ); 00148 */ 00149 void bayesianFusion(const CPose3DQuatPDF &p1,const CPose3DQuatPDF &p2 ); 00150 00151 /** Returns a new PDF such as: NEW_PDF = (0,0,0) - THIS_PDF 00152 */ 00153 void inverse(CPose3DQuatPDF &o) const; 00154 00155 /** Unary - operator, returns the PDF of the inverse pose. */ 00156 inline CPose3DQuatPDFGaussian operator -() const 00157 { 00158 CPose3DQuatPDFGaussian p(UNINITIALIZED_QUATERNION); 00159 this->inverse(p); 00160 return p; 00161 } 00162 00163 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated). 00164 */ 00165 void operator += ( const CPose3DQuat &Ap); 00166 00167 /** Makes: thisPDF = thisPDF + Ap, where "+" is pose composition (both the mean, and the covariance matrix are updated) (see formulas in jacobiansPoseComposition ). 00168 */ 00169 void operator += ( const CPose3DQuatPDFGaussian &Ap); 00170 00171 /** Makes: thisPDF = thisPDF - Ap, where "-" is pose inverse composition (both the mean, and the covariance matrix are updated). 00172 */ 00173 void operator -= ( const CPose3DQuatPDFGaussian &Ap); 00174 00175 /** Evaluates the PDF at a given point. 00176 */ 00177 double evaluatePDF( const CPose3DQuat &x ) const; 00178 00179 /** Evaluates the ratio PDF(x) / PDF(MEAN), that is, the normalized PDF in the range [0,1]. 00180 */ 00181 double evaluateNormalizedPDF( const CPose3DQuat &x ) const; 00182 00183 /** Computes the Mahalanobis distance between the centers of two Gaussians. 00184 * The variables with a variance exactly equal to 0 are not taken into account in the process, but 00185 * "infinity" is returned if the corresponding elements are not exactly equal. 00186 */ 00187 double mahalanobisDistanceTo( const CPose3DQuatPDFGaussian& theOther); 00188 00189 /** This static method computes the two Jacobians of a pose composition operation $f(x,u)= x \oplus u$ 00190 * \param out_x_oplus_u If set to !=NULL, the result of "x+u" will be stored here (it will be computed internally anyway). 00191 * To see the mathematical derivation of the formulas, refer to the technical report here: 00192 * - http://www.mrpt.org/Probability_Density_Distributions_Over_Spatial_Representations 00193 */ 00194 static void jacobiansPoseComposition( 00195 const CPose3DQuat &x, 00196 const CPose3DQuat &u, 00197 CMatrixDouble77 &df_dx, 00198 CMatrixDouble77 &df_du, 00199 CPose3DQuat *out_x_oplus_u=NULL); 00200 00201 00202 }; // End of class def. 00203 00204 00205 /** Pose composition for two 3D pose Gaussians \sa CPose3DQuatPDFGaussian::operator += */ 00206 inline CPose3DQuatPDFGaussian operator +( const CPose3DQuatPDFGaussian &x, const CPose3DQuatPDFGaussian &u ) 00207 { 00208 CPose3DQuatPDFGaussian res(x); 00209 res+=u; 00210 return res; 00211 } 00212 00213 /** Inverse pose composition for two 3D pose Gaussians \sa CPose3DQuatPDFGaussian::operator -= */ 00214 inline CPose3DQuatPDFGaussian operator -( const CPose3DQuatPDFGaussian &x, const CPose3DQuatPDFGaussian &u ) 00215 { 00216 CPose3DQuatPDFGaussian res(x); 00217 res-=u; 00218 return res; 00219 } 00220 00221 /** Dumps the mean and covariance matrix to a text stream. 00222 */ 00223 std::ostream BASE_IMPEXP & operator << (std::ostream & out, const CPose3DQuatPDFGaussian& obj); 00224 00225 bool BASE_IMPEXP operator==(const CPose3DQuatPDFGaussian &p1,const CPose3DQuatPDFGaussian &p2); 00226 00227 } // End of namespace 00228 00229 namespace global_settings 00230 { 00231 /** If set to true (default), a Scaled Unscented Transform is used instead of a linear approximation with Jacobians. 00232 * Affects to: 00233 * - CPose3DQuatPDFGaussian::copyFrom(const CPose3DPDFGaussian &o) 00234 */ 00235 extern BASE_IMPEXP bool USE_SUT_EULER2QUAT_CONVERSION; 00236 } 00237 00238 } // End of namespace 00239 00240 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |