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 CPose3DPDFSOG_H
00029 #define CPose3DPDFSOG_H
00030
00031 #include <mrpt/poses/CPose3DPDF.h>
00032 #include <mrpt/poses/CPose3DPDFGaussian.h>
00033 #include <mrpt/math/CMatrix.h>
00034
00035 namespace mrpt
00036 {
00037 namespace poses
00038 {
00039
00040 DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE( CPose3DPDFSOG, CPose3DPDF )
00041
00042
00053 class MRPTDLLIMPEXP CPose3DPDFSOG : public CPose3DPDF
00054 {
00055
00056 DEFINE_SERIALIZABLE( CPose3DPDFSOG )
00057
00058 protected:
00061 void assureSymmetry();
00062
00063 public:
00067 CPose3DPDFSOG( size_t nModes = 1 );
00068
00071 struct MRPTDLLIMPEXP TGaussianMode
00072 {
00073 TGaussianMode() :
00074 val(),
00075 log_w(0)
00076 { }
00077
00078 CPose3DPDFGaussian val;
00079
00082 double log_w;
00083 };
00084
00088 std::deque<TGaussianMode> m_modes;
00089
00092 CPose3D getEstimatedPose() const;
00093
00096 CMatrixD getEstimatedCovariance() const;
00097
00100 void normalizeWeights();
00101
00104 void copyFrom(const CPose3DPDF &o);
00105
00123 void saveToTextFile(const std::string &file) const;
00124
00128 void changeCoordinatesReference(const CPose3D &newReferenceBase );
00129
00132 void bayesianFusion( CPose3DPDF &p1, CPose3DPDF &p2 );
00133
00136 void drawSingleSample( CPose3D &outPart ) const;
00137
00140 void drawManySamples( size_t N, std::vector<vector_double> & outSamples ) const;
00141
00144 void inverse(CPose3DPDF &o) const;
00145
00148 void appendFrom( const CPose3DPDFSOG &o );
00149
00150 };
00151
00152
00153 }
00154 }
00155
00156 #endif