1 // -------------------------------------------------------------------------
2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
3 // -------------------------------------------------------------------------
5 #ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
6 #define __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
10 #include <vnl/vnl_math.h>
11 #include <vnl/vnl_inverse.h>
13 // -------------------------------------------------------------------------
14 template< class S, unsigned int D >
16 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
22 this->m_Cov.set_size( D, D );
23 this->m_Mean.set_size( D, 1 );
25 // Compute covariance matrix and mean vector
26 unsigned long N = this->m_Samples.size( );
27 this->m_Mean = this->m_S1;
29 this->m_Mean /= S( N );
32 this->m_Cov = this->m_S2;
33 this->m_Cov -= ( this->m_S1 * this->m_S1.transpose( ) ) / S( N );
34 this->m_Cov /= S( N - 1 );
37 this->m_Cov.fill( S( 0 ) );
39 // Compute inverse and determinant
40 S det = vnl_determinant( this->m_Cov );
41 if( !( det > S( 0 ) ) )
43 this->m_InvCov.set_size( D, D );
44 this->m_InvCov.fill( S( 0 ) );
45 this->m_DensityCoeff = S( 0 );
49 this->m_InvCov = vnl_inverse( this->m_Cov );
50 double _2piD = std::pow( double( 2 ) * double( vnl_math::pi ), D );
51 this->m_DensityCoeff = S( 1 ) / S( std::sqrt( _2piD * double( det ) ) );
55 // Compute minimum and maximum probabilities from input samples
57 for( unsigned long i = 0; i < this->m_Samples.size( ); ++i )
59 for( unsigned int d = 0; d < D; ++d )
60 sample[ d ] = this->m_Samples[ i ][ d ][ 0 ];
61 S p = this->Probability( sample );
64 this->m_MinimumProbability = p;
65 this->m_MaximumProbability = p;
69 if( p < this->m_MinimumProbability ) this->m_MinimumProbability = p;
70 if( this->m_MaximumProbability < p ) this->m_MaximumProbability = p;
75 this->m_Updated = true;
78 // -------------------------------------------------------------------------
79 template< class S, unsigned int D >
81 S cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
82 Probability( const V& sample ) const
85 for( unsigned int d = 0; d < D; ++d )
86 c[ d ][ 0 ] = S( sample[ d ] ) - this->m_Mean[ d ][ 0 ];
87 if( S( 0 ) < this->m_DensityCoeff )
89 // Covariance is NOT null
90 double v = double( ( c.transpose( ) * ( this->m_InvCov * c ) )[ 0 ][ 0 ] );
93 return( S( std::exp( -v ) ) );
99 for( unsigned int d = 0; d < D && equal; ++d )
100 equal &= !( S( 0 ) < S( std::fabs( double( c[ d ][ 0 ] ) ) ) );
101 return( ( equal )? S( 1 ): S( 0 ) );
106 // -------------------------------------------------------------------------
107 template< class S, unsigned int D >
108 S cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
109 Probability( const S& s_x, const S& s_y, ... ) const
111 static S sample[ D ];
113 std::va_list args_lst;
114 va_start( args_lst, s_y );
117 for( unsigned int d = 2; d < D; ++d )
118 sample[ d ] = S( va_arg( args_lst, double ) );
120 return( this->Probability( sample ) );
123 // -------------------------------------------------------------------------
124 template< class S, unsigned int D >
125 template< class V, class M >
127 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
128 GetModel( V& m, M& E ) const
130 for( unsigned int i = 0; i < D; ++i )
132 m[ i ] = double( this->m_Mean[ i ][ 0 ] );
133 for( unsigned int j = 0; j < D; ++j )
134 E[ i ][ j ] = double( this->m_Cov[ i ][ j ] );
139 // -------------------------------------------------------------------------
140 template< class S, unsigned int D >
142 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
145 this->m_Updated = false;
146 this->m_Samples.clear( );
147 this->m_S1.set_size( D, 1 );
148 this->m_S2.set_size( D, D );
149 this->m_S1.fill( S( 0 ) );
150 this->m_S2.fill( S( 0 ) );
154 // -------------------------------------------------------------------------
155 template< class S, unsigned int D >
158 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
159 AddSample( const V& sample )
162 for( unsigned int d = 0; d < D; ++d )
163 s[ d ][ 0 ] = S( sample[ d ] );
165 this->m_Samples.push_back( s );
167 this->m_S2 += s * s.transpose( );
169 this->m_Updated = false;
173 // -------------------------------------------------------------------------
174 template< class S, unsigned int D >
176 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
177 AddSample( const S& s_x, const S& s_y, ... )
179 static S sample[ D ];
181 std::va_list args_lst;
182 va_start( args_lst, s_y );
185 for( unsigned int d = 2; d < D; ++d )
186 sample[ d ] = S( va_arg( args_lst, double ) );
188 this->AddSample( sample );
191 // -------------------------------------------------------------------------
192 template< class S, unsigned int D >
193 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
194 IterativeGaussianModelEstimator( )
200 // -------------------------------------------------------------------------
201 template< class S, unsigned int D >
202 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
203 ~IterativeGaussianModelEstimator( )
207 #endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__