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 CPosePDFSOG_H
00029 #define CPosePDFSOG_H
00030
00031 #include <mrpt/poses/CPosePDF.h>
00032 #include <mrpt/math/CMatrix.h>
00033 #include <mrpt/math/CMatrixD.h>
00034
00035
00036 namespace mrpt
00037 {
00038 namespace opengl { struct CSetOfObjectsPtr; }
00039 namespace poses
00040 {
00041 using namespace mrpt::math;
00042
00043
00044 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPosePDFSOG , CPosePDF )
00045
00046
00057 class MRPTDLLIMPEXP CPosePDFSOG : public CPosePDF
00058 {
00059
00060 DEFINE_SERIALIZABLE( CPosePDFSOG )
00061
00062 protected:
00065 void assureSymmetry();
00066
00067 public:
00071 CPosePDFSOG( size_t nModes = 1 );
00072
00075 struct MRPTDLLIMPEXP TGaussianMode
00076 {
00077 TGaussianMode() :
00078 mean(),
00079 cov(3,3),
00080 log_w(0)
00081 { }
00082
00083 CPose2D mean;
00084 CMatrixD cov;
00085
00088 double log_w;
00089 };
00090
00094 std::deque<TGaussianMode> m_modes;
00095
00097 size_t size() const { return m_modes.size(); }
00098
00105 void mergeModes( double max_KLd = 0.5, bool verbose = false );
00106
00109 CPose2D getEstimatedPose() const;
00110
00113 CMatrixD getEstimatedCovariance() const;
00114
00117 void normalizeWeights();
00118
00121 void copyFrom(const CPosePDF &o);
00122
00137 void saveToTextFile(const std::string &file) const;
00138
00142 void changeCoordinatesReference(const CPose2D &newReferenceBase );
00143
00146 void rotateAllCovariances(const double &ang);
00147
00150 void drawSingleSample( CPose2D &outPart ) const;
00151
00154 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const;
00155
00158 void inverse(CPosePDF &o) const;
00159
00162 void operator += ( CPose2D Ap);
00163
00166 double evaluatePDF( const CPose2D &x, bool sumOverAllPhis = false ) const;
00167
00170 double evaluateNormalizedPDF( const CPose2D &x ) const;
00171
00174 void evaluatePDFInArea(
00175 const double & x_min,
00176 const double & x_max,
00177 const double & y_min,
00178 const double & y_max,
00179 const double & resolutionXY,
00180 const double & phi,
00181 CMatrixD &outMatrix,
00182 bool sumOverAllPhis = false );
00183
00186 void bayesianFusion( CPosePDF &p1, CPosePDF &p2, const double &minMahalanobisDistToDrop=0 );
00187
00188
00191 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00192
00193
00194 };
00195
00196 }
00197 }
00198
00199 #endif