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__
11 #include <vnl/vnl_math.h>
12 #include <vnl/vnl_inverse.h>
14 // -------------------------------------------------------------------------
15 template< class S, unsigned int D >
17 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
18 SetNumberOfSamples( unsigned long n )
21 this->m_Updated = false;
25 // -------------------------------------------------------------------------
26 template< class S, unsigned int D >
28 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
29 SetMu( const TMatrix& m )
31 if( m.rows( ) == D && m.columns( ) == 1 )
34 this->m_Updated = false;
40 << "Input Mu matrix is not a " << D << "x1 matrix"
46 // -------------------------------------------------------------------------
47 template< class S, unsigned int D >
49 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
50 SetOmega( const TMatrix& O )
52 if( O.rows( ) == D && O.columns( ) == D )
55 this->m_Updated = false;
61 << "Input Omega matrix is not a " << D << "x" << D << " matrix"
67 // -------------------------------------------------------------------------
68 template< class S, unsigned int D >
70 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
71 SaveModelToFile( const std::string& filename ) const
73 std::ofstream out( filename.c_str( ) );
76 out << this->m_N << std::endl;
77 out << this->m_M << std::endl;
78 out << this->m_O << std::endl;
86 // -------------------------------------------------------------------------
87 template< class S, unsigned int D >
89 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
90 LoadModelFromFile( const std::string& filename )
92 std::ifstream in( filename.c_str( ) );
97 for( unsigned int i = 0; i < D; ++i )
98 in >> this->m_M[ i ][ 0 ];
99 for( unsigned int j = 0; j < D; ++j )
100 for( unsigned int i = 0; i < D; ++i )
101 in >> this->m_O[ i ][ j ];
109 // -------------------------------------------------------------------------
110 template< class S, unsigned int D >
112 S cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
113 Probability( const V& sample ) const
115 if( !this->m_Updated )
116 this->_UpdateModel( );
119 for( unsigned int d = 0; d < D; ++d )
120 c[ d ][ 0 ] = S( sample[ d ] ) - this->m_M[ d ][ 0 ];
121 if( S( 0 ) < this->m_Norm )
123 // Covariance is NOT null
124 double v = double( ( c.transpose( ) * ( this->m_Inv * c ) )[ 0 ][ 0 ] );
127 return( S( std::exp( -v ) ) );
131 // Covariance is null
133 for( unsigned int d = 0; d < D; ++d )
134 n += c[ d ][ 0 ] * c[ d ][ 0 ];
135 bool equal = ( double( n ) < double( 1e-10 ) );
136 return( ( equal )? S( 1 ): S( 0 ) );
141 // -------------------------------------------------------------------------
142 template< class S, unsigned int D >
143 S cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
144 Probability( const S& s_x, const S& s_y, ... ) const
146 static S sample[ D ];
148 std::va_list args_lst;
149 va_start( args_lst, s_y );
152 for( unsigned int d = 2; d < D; ++d )
153 sample[ d ] = S( va_arg( args_lst, double ) );
155 return( this->Probability( sample ) );
158 // -------------------------------------------------------------------------
159 template< class S, unsigned int D >
160 template< class V, class M >
162 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
163 GetModel( V& m, M& E ) const
165 if( !this->m_Updated )
166 this->_UpdateModel( );
167 for( unsigned int i = 0; i < D; ++i )
169 m[ i ] = double( this->m_M[ i ][ 0 ] );
170 for( unsigned int j = 0; j < D; ++j )
171 E[ i ][ j ] = double( this->m_Cov[ i ][ j ] );
176 // -------------------------------------------------------------------------
177 template< class S, unsigned int D >
179 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
183 this->m_M.set_size( D, 1 );
184 this->m_O.set_size( D, D );
185 this->m_M.fill( S( 0 ) );
186 this->m_O.fill( S( 0 ) );
187 this->m_Updated = false;
191 // -------------------------------------------------------------------------
192 template< class S, unsigned int D >
195 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
196 AddSample( const V& sample )
199 for( unsigned int d = 0; d < D; ++d )
200 s[ d ][ 0 ] = S( sample[ d ] );
206 this->m_M = ( this->m_M * coeff ) + ( s / this->m_N );
208 // Update omega operand
210 this->m_O = s * s.transpose( );
211 else if( this->m_N == 2 )
212 this->m_O += s * s.transpose( );
215 S inv = S( 1 ) / ( this->m_N - S( 1 ) );
216 this->m_O = this->m_O * ( this->m_N - S( 2 ) ) * inv;
217 this->m_O += ( s * s.transpose( ) ) * inv;
221 this->m_Updated = false;
225 // -------------------------------------------------------------------------
226 template< class S, unsigned int D >
228 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
229 AddSample( const S& s_x, const S& s_y, ... )
231 static S sample[ D ];
233 std::va_list args_lst;
234 va_start( args_lst, s_y );
239 for( unsigned int d = 2; d < D; ++d )
240 sample[ d ] = S( va_arg( args_lst, double ) );
244 this->AddSample( sample );
247 // -------------------------------------------------------------------------
248 template< class S, unsigned int D >
249 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
250 IterativeGaussianModelEstimator( )
256 // -------------------------------------------------------------------------
257 template< class S, unsigned int D >
258 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
259 ~IterativeGaussianModelEstimator( )
263 // -------------------------------------------------------------------------
264 template< class S, unsigned int D >
266 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
267 _UpdateModel( ) const
269 static const double _2piD =
270 std::pow( double( 2 ) * double( vnl_math::pi ), D );
276 ( this->m_M * this->m_M.transpose( ) ) *
277 ( this->m_N / ( this->m_N - S( 1 ) ) )
280 // Compute inverse and determinant
281 S det = vnl_determinant( this->m_Cov );
282 if( !( det > S( 0 ) ) )
284 this->m_Inv.set_size( D, D );
285 this->m_Inv.fill( S( 0 ) );
286 this->m_Norm = S( 0 );
290 this->m_Inv = vnl_inverse( this->m_Cov );
291 this->m_Norm = S( 1 ) / S( std::sqrt( _2piD * double( det ) ) );
295 // Object now is updated
296 this->m_Updated = true;
299 #endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__