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 CPointPDFSOG_H
00029 #define CPointPDFSOG_H
00030
00031 #include <mrpt/poses/CPointPDF.h>
00032 #include <mrpt/poses/CPointPDFGaussian.h>
00033 #include <mrpt/math/CMatrix.h>
00034 #include <mrpt/math/CMatrixD.h>
00035
00036 namespace mrpt
00037 {
00038 namespace opengl
00039 {
00040 struct CSetOfObjectsPtr;
00041 }
00042 namespace poses
00043 {
00044 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointPDFSOG, CPointPDF )
00045
00046
00057 class MRPTDLLIMPEXP CPointPDFSOG : public CPointPDF
00058 {
00059
00060 DEFINE_SERIALIZABLE( CPointPDFSOG )
00061
00062 protected:
00065 void assureSymmetry();
00066
00067 public:
00071 CPointPDFSOG( size_t nModes = 1 );
00072
00075 struct MRPTDLLIMPEXP TGaussianMode
00076 {
00077 TGaussianMode() : val(), log_w(0)
00078 {
00079 }
00080
00081 CPointPDFGaussian val;
00082
00085 double log_w;
00086 };
00087
00088
00089 typedef std::vector<TGaussianMode> CListGaussianModes;
00090
00094 CListGaussianModes m_modes;
00095
00098 size_t size() const
00099 {
00100 return m_modes.size();
00101 }
00102
00105 CPoint3D getEstimatedPoint() const;
00106
00109 CMatrixD getEstimatedCovariance() const;
00110
00113 void normalizeWeights();
00114
00117 double ESS() const;
00118
00121 void copyFrom(const CPointPDF &o);
00122
00137 void saveToTextFile(const std::string &file) const;
00138
00142 void changeCoordinatesReference(const CPose3D &newReferenceBase );
00143
00146 void getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const;
00147
00150 void drawSingleSample(CPoint3D &outSample) const;
00151
00157 void bayesianFusion( CPointPDF &p1, CPointPDF &p2, const double &minMahalanobisDistToDrop = 0);
00158
00159
00162 void evaluatePDFInArea(
00163 float x_min,
00164 float x_max,
00165 float y_min,
00166 float y_max,
00167 float resolutionXY,
00168 float z,
00169 CMatrixD &outMatrix,
00170 bool sumOverAllZs = false );
00171
00174 double evaluatePDF(
00175 const CPoint3D &x,
00176 bool sumOverAllZs ) const;
00177
00178
00179 };
00180
00181
00182 }
00183 }
00184
00185 #endif