lib/cpPlugins/Extensions/Algorithms/IterativeGaussianModelEstimator.hxx

   1 // -------------------------------------------------------------------------
   2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
   3 // -------------------------------------------------------------------------
   4
   5 #ifndef __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
   6 #define __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
   7
   8 #include <cmath>
   9 #include <cstdarg>
  10 #include <vnl/vnl_math.h>
  11 #include <vnl/vnl_inverse.h>
  12
  13 // -------------------------------------------------------------------------
  14 template< class S, unsigned int D >
  15 void
  16 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
  17 UpdateModel( )
  18 {
  19   if( this->m_Updated )
  20     return;
  21
  22   this->m_Cov.set_size( D, D );
  23   this->m_Mean.set_size( D, 1 );
  24
  25   // Compute covariance matrix and mean vector
  26   unsigned long N = this->m_Samples.size( );
  27   this->m_Mean = this->m_S1;
  28   if( N > 0 )
  29     this->m_Mean /= S( N );
  30   if( N > 1 )
  31   {
  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 );
  35   }
  36   else
  37     this->m_Cov.fill( S( 0 ) );
  38
  39   // Compute inverse and determinant
  40   S det = vnl_determinant( this->m_Cov );
  41   if( !( det > S( 0 ) ) )
  42   {
  43     this->m_InvCov.set_size( D, D );
  44     this->m_InvCov.fill( S( 0 ) );
  45     this->m_DensityCoeff = S( 0 );
  46   }
  47   else
  48   {
  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 ) ) );
  52
  53   } // fi
  54
  55   // Compute minimum and maximum probabilities from input samples
  56   static S sample[ D ];
  57   for( unsigned long i = 0; i < this->m_Samples.size( ); ++i )
  58   {
  59     for( unsigned int d = 0; d < D; ++d )
  60       sample[ d ] = this->m_Samples[ i ][ d ][ 0 ];
  61     S p = this->Probability( sample );
  62     if( i == 0 )
  63     {
  64       this->m_MinimumProbability = p;
  65       this->m_MaximumProbability = p;
  66     }
  67     else
  68     {
  69       if( p < this->m_MinimumProbability ) this->m_MinimumProbability = p;
  70       if( this->m_MaximumProbability < p ) this->m_MaximumProbability = p;
  71
  72     } // fi
  73
  74   } // rof
  75   this->m_Updated = true;
  76 }
  77
  78 // -------------------------------------------------------------------------
  79 template< class S, unsigned int D >
  80 template< class V >
  81 S cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
  82 Probability( const V& sample ) const
  83 {
  84   TMatrix c( D, 1 );
  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 )
  88   {
  89     // Covariance is NOT null
  90     double v = double( ( c.transpose( ) * ( this->m_InvCov * c ) )[ 0 ][ 0 ] );
  91     v /= double( 2 );
  92
  93     return( S( std::exp( -v ) ) );
  94   }
  95   else
  96   {
  97     // Covariance is null
  98     bool equal = true;
  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 ) );
 102
 103   } // fi
 104 }
 105
 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
 110 {
 111   static S sample[ D ];
 112
 113   std::va_list args_lst;
 114   va_start( args_lst, s_y );
 115   sample[ 0 ] = s_x;
 116   sample[ 1 ] = s_y;
 117   for( unsigned int d = 2; d < D; ++d )
 118     sample[ d ] = S( va_arg( args_lst, double ) );
 119   va_end( args_lst );
 120   return( this->Probability( sample ) );
 121 }
 122
 123 // -------------------------------------------------------------------------
 124 template< class S, unsigned int D >
 125 template< class V, class M >
 126 void
 127 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 128 GetModel( V& m, M& E ) const
 129 {
 130   for( unsigned int i = 0; i < D; ++i )
 131   {
 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 ] );
 135
 136   } // rof
 137 }
 138
 139 // -------------------------------------------------------------------------
 140 template< class S, unsigned int D >
 141 void
 142 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 143 Clear( )
 144 {
 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 ) );
 151   this->Modified( );
 152 }
 153
 154 // -------------------------------------------------------------------------
 155 template< class S, unsigned int D >
 156 template< class V >
 157 void
 158 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 159 AddSample( const V& sample )
 160 {
 161   TMatrix s( D, 1 );
 162   for( unsigned int d = 0; d < D; ++d )
 163     s[ d ][ 0 ] = S( sample[ d ] );
 164
 165   this->m_Samples.push_back( s );
 166   this->m_S1 += s;
 167   this->m_S2 += s * s.transpose( );
 168
 169   this->m_Updated = false;
 170   this->Modified( );
 171 }
 172
 173 // -------------------------------------------------------------------------
 174 template< class S, unsigned int D >
 175 void
 176 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 177 AddSample( const S& s_x, const S& s_y, ... )
 178 {
 179   static S sample[ D ];
 180
 181   std::va_list args_lst;
 182   va_start( args_lst, s_y );
 183   sample[ 0 ] = s_x;
 184   sample[ 1 ] = s_y;
 185   for( unsigned int d = 2; d < D; ++d )
 186     sample[ d ] = S( va_arg( args_lst, double ) );
 187   va_end( args_lst );
 188   this->AddSample( sample );
 189 }
 190
 191 // -------------------------------------------------------------------------
 192 template< class S, unsigned int D >
 193 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 194 IterativeGaussianModelEstimator( )
 195   : Superclass( )
 196 {
 197   this->Clear( );
 198 }
 199
 200 // -------------------------------------------------------------------------
 201 template< class S, unsigned int D >
 202 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 203 ~IterativeGaussianModelEstimator( )
 204 {
 205 }
 206
 207 #endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
 208
 209 // eof - $RCSfile$