1 // -------------------------------------------------------------------------
2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
3 // -------------------------------------------------------------------------
5 #ifndef __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
6 #define __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
11 #include <vnl/vnl_math.h>
12 #include <vnl/vnl_inverse.h>
14 // -------------------------------------------------------------------------
15 template< class S, unsigned int D >
16 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
17 SetNumberOfSamples( unsigned long n )
20 this->m_Updated = false;
24 // -------------------------------------------------------------------------
25 template< class S, unsigned int D >
26 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
27 SetMu( const TMatrix& m )
29 if( m.rows( ) == D && m.columns( ) == 1 )
32 this->m_Updated = false;
38 << "Input Mu matrix is not a " << D << "x1 matrix"
44 // -------------------------------------------------------------------------
45 template< class S, unsigned int D >
46 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
47 SetOmega( const TMatrix& O )
49 if( O.rows( ) == D && O.columns( ) == D )
52 this->m_Updated = false;
58 << "Input Omega matrix is not a " << D << "x" << D << " matrix"
64 // -------------------------------------------------------------------------
65 template< class S, unsigned int D >
66 bool cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
67 SaveModelToFile( const std::string& filename ) const
69 std::ofstream out( filename.c_str( ) );
72 out << this->m_N << std::endl;
73 out << this->m_M << std::endl;
74 out << this->m_O << std::endl;
82 // -------------------------------------------------------------------------
83 template< class S, unsigned int D >
84 bool cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
85 LoadModelFromFile( const std::string& filename )
87 std::ifstream in( filename.c_str( ) );
92 for( unsigned int i = 0; i < D; ++i )
93 in >> this->m_M[ i ][ 0 ];
94 for( unsigned int j = 0; j < D; ++j )
95 for( unsigned int i = 0; i < D; ++i )
96 in >> this->m_O[ i ][ j ];
104 // -------------------------------------------------------------------------
105 template< class S, unsigned int D >
107 S cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
108 Probability( const V& sample ) const
110 if( !this->m_Updated )
111 this->_UpdateModel( );
114 for( unsigned int d = 0; d < D; ++d )
115 c[ d ][ 0 ] = S( sample[ d ] ) - this->m_M[ d ][ 0 ];
116 if( S( 0 ) < this->m_Norm )
118 // Covariance is NOT null
119 double v = double( ( c.transpose( ) * ( this->m_Inv * c ) )[ 0 ][ 0 ] );
122 return( S( std::exp( -v ) ) );
126 // Covariance is null
128 for( unsigned int d = 0; d < D; ++d )
129 n += c[ d ][ 0 ] * c[ d ][ 0 ];
130 bool equal = ( double( n ) < double( 1e-10 ) );
131 return( ( equal )? S( 1 ): S( 0 ) );
136 // -------------------------------------------------------------------------
137 template< class S, unsigned int D >
138 S cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
139 Probability( const S& s_x, const S& s_y, ... ) const
141 static S sample[ D ];
143 std::va_list args_lst;
144 va_start( args_lst, s_y );
147 for( unsigned int d = 2; d < D; ++d )
148 sample[ d ] = S( va_arg( args_lst, double ) );
150 return( this->Probability( sample ) );
153 // -------------------------------------------------------------------------
154 template< class S, unsigned int D >
155 template< class V, class M >
156 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
157 GetModel( V& m, M& E ) const
159 if( !this->m_Updated )
160 this->_UpdateModel( );
161 for( unsigned int i = 0; i < D; ++i )
163 m[ i ] = double( this->m_M[ i ][ 0 ] );
164 for( unsigned int j = 0; j < D; ++j )
165 E[ i ][ j ] = double( this->m_Cov[ i ][ j ] );
170 // -------------------------------------------------------------------------
171 template< class S, unsigned int D >
172 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
176 this->m_M.set_size( D, 1 );
177 this->m_O.set_size( D, D );
178 this->m_M.fill( S( 0 ) );
179 this->m_O.fill( S( 0 ) );
180 this->m_Updated = false;
184 // -------------------------------------------------------------------------
185 template< class S, unsigned int D >
187 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
188 AddSample( const V& sample )
191 for( unsigned int d = 0; d < D; ++d )
192 s[ d ][ 0 ] = S( sample[ d ] );
198 this->m_M = ( this->m_M * coeff ) + ( s / this->m_N );
200 // Update omega operand
202 this->m_O = s * s.transpose( );
203 else if( this->m_N == 2 )
204 this->m_O += s * s.transpose( );
207 S inv = S( 1 ) / ( this->m_N - S( 1 ) );
208 this->m_O = this->m_O * ( this->m_N - S( 2 ) ) * inv;
209 this->m_O += ( s * s.transpose( ) ) * inv;
213 this->m_Updated = false;
217 // -------------------------------------------------------------------------
218 template< class S, unsigned int D >
219 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
220 AddSample( const S& s_x, const S& s_y, ... )
222 static S sample[ D ];
224 std::va_list args_lst;
225 va_start( args_lst, s_y );
230 for( unsigned int d = 2; d < D; ++d )
231 sample[ d ] = S( va_arg( args_lst, double ) );
235 this->AddSample( sample );
238 // -------------------------------------------------------------------------
239 template< class S, unsigned int D >
240 cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
241 IterativeGaussianModelEstimator( )
247 // -------------------------------------------------------------------------
248 template< class S, unsigned int D >
249 cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
250 ~IterativeGaussianModelEstimator( )
254 // -------------------------------------------------------------------------
255 template< class S, unsigned int D >
256 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
257 _UpdateModel( ) const
259 static const double _2piD =
260 std::pow( double( 2 ) * double( vnl_math::pi ), D );
266 ( this->m_M * this->m_M.transpose( ) ) *
267 ( this->m_N / ( this->m_N - S( 1 ) ) )
270 // Compute inverse and determinant
271 S det = vnl_determinant( this->m_Cov );
272 if( !( det > S( 0 ) ) )
274 this->m_Inv.set_size( D, D );
275 this->m_Inv.fill( S( 0 ) );
276 this->m_Norm = S( 0 );
280 this->m_Inv = vnl_inverse( this->m_Cov );
281 this->m_Norm = S( 1 ) / S( std::sqrt( _2piD * double( det ) ) );
285 // Object now is updated
286 this->m_Updated = true;
289 #endif // __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__