#ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
#define __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
+#include <cmath>
#include <cstdarg>
+#include <vnl/vnl_math.h>
+#include <vnl/vnl_inverse.h>
// -------------------------------------------------------------------------
template< class S, unsigned int D >
-template< class V, class M >
void
cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-GetModel( V& m, M& E ) const
+UpdateModel( )
{
- TMatrix Es = ( this->m_S1 * this->m_S1.transpose( ) );
-
- for( unsigned int i = 0; i < D; ++i )
+ if( this->m_Updated )
+ return;
+
+ this->m_Cov.set_size( D, D );
+ this->m_Mean.set_size( D, 1 );
+
+ // Compute covariance matrix and mean vector
+ unsigned long N = this->m_Samples.size( );
+ this->m_Mean = this->m_S1;
+ if( N > 0 )
+ this->m_Mean /= S( N );
+ if( N > 1 )
{
- m[ i ] = this->m_S1[ i ][ 0 ];
- if( this->m_N > 0 )
- m[ i ] /= double( this->m_N );
+ this->m_Cov = this->m_S2;
+ this->m_Cov -= ( this->m_S1 * this->m_S1.transpose( ) ) / S( N );
+ this->m_Cov /= S( N - 1 );
+ }
+ else
+ this->m_Cov.fill( S( 0 ) );
+
+ /* TODO
+ TMatrix Es = ( this->m_S1 * this->m_S1.transpose( ) );
+ for( unsigned int i = 0; i < D; ++i )
+ {
+ this->m_Mean[ i ][ 0 ] = this->m_S1[ i ][ 0 ];
+ if( this->m_N > 0 )
+ this->m_Mean[ i ][ 0 ] /= S( this->m_N );
+
+ for( unsigned int j = 0; j < D; ++j )
+ {
+ this->m_Cov[ i ][ j ] = S( 0 );
+ if( this->m_N > 0 )
+ this->m_Cov[ i ][ j ] -= Es[ i ][ j ] / S( this->m_N );
+ if( this->m_N > 1 )
+ {
+ this->m_Cov[ i ][ j ] += this->m_S2[ i ][ j ];
+ this->m_Cov[ i ][ j ] /= S( this->m_N - 1 );
+
+ } // fi
+
+ } // rof
+
+ } // rof
+ */
+
+ // Compute inverse and determinant
+ S det = vnl_determinant( this->m_Cov );
+ if( !( det > S( 0 ) ) )
+ {
+ this->m_InvCov.set_size( D, D );
+ this->m_InvCov.fill( S( 0 ) );
+ this->m_DensityCoeff = S( 0 );
+ }
+ else
+ {
+ this->m_InvCov = vnl_inverse( this->m_Cov );
+ double _2piD = std::pow( double( 2 ) * double( vnl_math::pi ), D );
+ this->m_DensityCoeff = S( 1 ) / S( std::sqrt( _2piD * double( det ) ) );
- for( unsigned int j = 0; j < D; ++j )
+ } // fi
+
+ // Compute minimum and maximum probabilities from input samples
+ static S sample[ D ];
+ for( unsigned long i = 0; i < this->m_Samples.size( ); ++i )
+ {
+ for( unsigned int d = 0; d < D; ++d )
+ sample[ d ] = this->m_Samples[ i ][ d ][ 0 ];
+ S p = this->Probability( sample );
+ if( i == 0 )
{
- E[ i ][ j ] = double( 0 );
- if( this->m_N > 0 )
- E[ i ][ j ] -= double( Es[ i ][ j ] / S( this->m_N ) );
- if( this->m_N > 1 )
- {
- E[ i ][ j ] += double( this->m_S2[ i ][ j ] );
- E[ i ][ j ] /= double( this->m_N - 1 );
+ this->m_MinimumProbability = p;
+ this->m_MaximumProbability = p;
+ }
+ else
+ {
+ if( p < this->m_MinimumProbability ) this->m_MinimumProbability = p;
+ if( this->m_MaximumProbability < p ) this->m_MaximumProbability = p;
+
+ } // fi
+
+ } // rof
+ this->m_Updated = true;
+
+ /* DEBUG:
+ std::cout << "--------------------" << std::endl;
+ std::cout << this->m_Cov << std::endl;
+ std::cout << "--------------------" << std::endl;
+ std::cout << this->m_InvCov << std::endl;
+ std::cout << "--------------------" << std::endl;
+ std::cout << this->m_Mean << std::endl;
+ */
+}
- } // fi
+// -------------------------------------------------------------------------
+template< class S, unsigned int D >
+template< class V >
+S cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
+Probability( const V& sample ) const
+{
+ TMatrix c( D, 1 );
+ for( unsigned int d = 0; d < D; ++d )
+ c[ d ][ 0 ] = S( sample[ d ] ) - this->m_Mean[ d ][ 0 ];
+ double v = double( ( c.transpose( ) * ( this->m_InvCov * c ) )[ 0 ][ 0 ] );
+ v /= double( 2 );
- } // rof
+ return( S( std::exp( -v ) ) );
+}
+
+// -------------------------------------------------------------------------
+template< class S, unsigned int D >
+S cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
+Probability( const S& s_x, const S& s_y, ... ) const
+{
+ static S sample[ D ];
+
+ std::va_list args_lst;
+ va_start( args_lst, s_y );
+ sample[ 0 ] = s_x;
+ sample[ 1 ] = s_y;
+ for( unsigned int d = 2; d < D; ++d )
+ sample[ d ] = S( va_arg( args_lst, double ) );
+ va_end( args_lst );
+ return( this->Probability( sample ) );
+}
+
+// -------------------------------------------------------------------------
+template< class S, unsigned int D >
+template< class V, class M >
+void
+cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
+GetModel( V& m, M& E ) const
+{
+ for( unsigned int i = 0; i < D; ++i )
+ {
+ m[ i ] = double( this->m_Mean[ i ][ 0 ] );
+ for( unsigned int j = 0; j < D; ++j )
+ E[ i ][ j ] = double( this->m_Cov[ i ][ j ] );
} // rof
}
cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
Clear( )
{
- this->m_N = 0;
+ this->m_Updated = false;
+ this->m_Samples.clear( );
this->m_S1.set_size( D, 1 );
this->m_S2.set_size( D, D );
this->m_S1.fill( S( 0 ) );
cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
AddSample( const V& sample )
{
- for( unsigned int i = 0; i < D; ++i )
- {
- this->m_S1[ i ][ 0 ] += S( sample[ i ] );
- for( unsigned int j = i; j < D; ++j )
- {
- this->m_S2[ i ][ j ] += S( sample[ i ] ) * S( sample[ j ] );
- this->m_S2[ j ][ i ] = this->m_S2[ i ][ j ];
-
- } // rof
-
- } // rof
- this->m_N++;
+ TMatrix s( D, 1 );
+ for( unsigned int d = 0; d < D; ++d )
+ s[ d ][ 0 ] = S( sample[ d ] );
+
+ this->m_Samples.push_back( s );
+ this->m_S1 += s;
+ this->m_S2 += s * s.transpose( );
+
+ /* DEBUG:
+ std::cout
+ << sample[ 0 ] << " " << sample[ 1 ] << " " << sample[ 2 ] << std::endl;
+ */
+
+ /* TODO
+ {
+ this->m_S1[ i ][ 0 ] += S( sample[ i ] );
+ for( unsigned int j = i; j < D; ++j )
+ {
+ this->m_S2[ i ][ j ] += S( sample[ i ] ) * S( sample[ j ] );
+ this->m_S2[ j ][ i ] = this->m_S2[ i ][ j ];
+
+ } // rof
+
+ } // rof
+ */
+ this->m_Updated = false;
this->Modified( );
}
git://git.creatis.insa-lyon.fr / cpPlugins.git / blobdiff