00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef CPosePDFGaussian_H
00029 #define CPosePDFGaussian_H
00030
00031 #include <mrpt/poses/CPosePDF.h>
00032 #include <mrpt/math/CMatrixD.h>
00033
00034 namespace mrpt
00035 {
00036 namespace poses
00037 {
00038 using namespace mrpt::math;
00039
00040 class CPose3DPDF;
00041
00042
00043 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFGaussian, CPosePDF )
00044
00045
00051 class MRPTDLLIMPEXP CPosePDFGaussian : public CPosePDF
00052 {
00053
00054 DEFINE_SERIALIZABLE( CPosePDFGaussian )
00055
00056 protected:
00059 void assureSymmetry();
00060
00061 public:
00064 CPosePDFGaussian();
00065
00068 CPosePDFGaussian( const CPose2D &init_Mean );
00069
00072 CPosePDFGaussian( const CPose2D &init_Mean, const CMatrixD &init_Cov );
00073
00075 CPosePDFGaussian( const CPosePDF &o ) { copyFrom( o ); }
00076
00078 CPosePDFGaussian( const CPose3DPDF &o ) { copyFrom( o ); }
00079
00082 CPose2D mean;
00083
00086 CMatrixD cov;
00087
00088
00091 CPose2D getEstimatedPose() const;
00092
00095 CMatrixD getEstimatedCovariance() const;
00096
00099 void copyFrom(const CPosePDF &o);
00100
00103 void copyFrom(const CPose3DPDF &o);
00104
00107 void saveToTextFile(const std::string &file) const;
00108
00112 void changeCoordinatesReference( const CPose2D &newReferenceBase );
00113
00116 void rotateCov(const double& ang);
00117
00120 void inverseComposition( const CPosePDFGaussian &x, const CPosePDFGaussian &ref );
00121
00124 void inverseComposition(
00125 const CPosePDFGaussian &x1,
00126 const CPosePDFGaussian &x0,
00127 const CMatrixDouble &COV_01
00128 );
00129
00132 void drawSingleSample( CPose2D &outPart ) const;
00133
00136 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const;
00137
00147 void bayesianFusion( CPosePDF &p1, CPosePDF &p2, const double &minMahalanobisDistToDrop = 0 );
00148
00151 void inverse(CPosePDF &o) const;
00152
00155 void operator += ( CPose2D Ap);
00156
00159 double evaluatePDF( const CPose2D &x ) const;
00160
00163 double evaluateNormalizedPDF( const CPose2D &x ) const;
00164
00167 double mahalanobisDistanceTo( const CPosePDFGaussian& theOther );
00168
00171 void assureMinCovariance( const double & minStdXY, const double &minStdPhi );
00172
00173 };
00174
00175
00176
00179 std::ostream & operator << (std::ostream & out, const CPosePDFGaussian& obj);
00180
00183 poses::CPosePDFGaussian operator + ( const mrpt::poses::CPose2D &A, const mrpt::poses::CPosePDFGaussian &B );
00184
00185 }
00186 }
00187
00188 #endif