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 CPointPDFParticles_H
00029 #define CPointPDFParticles_H
00030
00031 #include <mrpt/poses/CPointPDF.h>
00032 #include <mrpt/math/CMatrix.h>
00033 #include <mrpt/bayes/CProbabilityParticle.h>
00034 #include <mrpt/bayes/CParticleFilterData.h>
00035
00036 namespace mrpt
00037 {
00038 namespace poses
00039 {
00040 DEFINE_SERIALIZABLE_PRE( TSimple3DPoint )
00041
00042
00044 class MRPTDLLIMPEXP TSimple3DPoint : public mrpt::utils::CSerializable
00045 {
00046
00047 DEFINE_SERIALIZABLE( TSimple3DPoint )
00048 public:
00049 TSimple3DPoint(const TSimple3DPoint&o) : x(o.x),y(o.y),z(o.z)
00050 {
00051 }
00052
00053 TSimple3DPoint() : x(0),y(0),z(0)
00054 {
00055 }
00056
00057 TSimple3DPoint(const CPoint3D &v) : x(v.x),y(v.y),z(v.z)
00058 {
00059 }
00060
00061 float x,y,z;
00062 };
00063
00064 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPointPDFParticles, CPointPDF )
00065
00066
00069 class MRPTDLLIMPEXP CPointPDFParticles : public CPointPDF, public mrpt::bayes::CParticleFilterData<TSimple3DPoint>
00070 {
00071
00072 DEFINE_SERIALIZABLE( CPointPDFParticles )
00073
00074
00075 IMPLEMENT_PARTICLE_FILTER_CAPABLE(TSimple3DPoint);
00076
00077 public:
00080 CPointPDFParticles(size_t numParticles = 1);
00081
00084
00085
00088
00089
00092 virtual ~CPointPDFParticles();
00093
00096 void clear() { setSize(0); }
00097
00100 void setSize(size_t numberParticles, const CPoint3D &defaultValue = CPoint3D(0,0,0) );
00101
00104 size_t size() const
00105 {
00106 return m_particles.size();
00107 }
00108
00111 CPoint3D getEstimatedPoint() const;
00112
00115 CMatrixD getEstimatedCovariance() const;
00116
00119 void copyFrom(const CPointPDF &o);
00120
00123 void saveToTextFile(const std::string &file) const;
00124
00128 void changeCoordinatesReference( const CPose3D &newReferenceBase );
00129
00132 double computeKurtosis();
00133
00136 void drawSingleSample(CPoint3D &outSample) const;
00137
00143 void bayesianFusion( CPointPDF &p1, CPointPDF &p2, const double &minMahalanobisDistToDrop = 0);
00144
00145 };
00146
00147
00148 }
00149 }
00150
00151 #endif