Main MRPT website > C++ reference
MRPT logo

CPose3DPDFGaussian.h

Go to the documentation of this file.
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:exported at Tue Jan 25 21:56:31 UTC 2011