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   /* TODO
  40      TMatrix Es = ( this->m_S1 * this->m_S1.transpose( ) );
  41      for( unsigned int i = 0; i < D; ++i )
  42      {
  43      this->m_Mean[ i ][ 0 ] = this->m_S1[ i ][ 0 ];
  44      if( this->m_N > 0 )
  45      this->m_Mean[ i ][ 0 ] /= S( this->m_N );
  46
  47      for( unsigned int j = 0; j < D; ++j )
  48      {
  49      this->m_Cov[ i ][ j ] = S( 0 );
  50      if( this->m_N > 0 )
  51      this->m_Cov[ i ][ j ] -= Es[ i ][ j ] / S( this->m_N );
  52      if( this->m_N > 1 )
  53      {
  54      this->m_Cov[ i ][ j ] += this->m_S2[ i ][ j ];
  55      this->m_Cov[ i ][ j ] /= S( this->m_N - 1 );
  56
  57      } // fi
  58
  59      } // rof
  60
  61      } // rof
  62   */
  63
  64   // Compute inverse and determinant
  65   S det = vnl_determinant( this->m_Cov );
  66   if( !( det > S( 0 ) ) )
  67   {
  68     this->m_InvCov.set_size( D, D );
  69     this->m_InvCov.fill( S( 0 ) );
  70     this->m_DensityCoeff = S( 0 );
  71   }
  72   else
  73   {
  74     this->m_InvCov = vnl_inverse( this->m_Cov );
  75     double _2piD = std::pow( double( 2 ) * double( vnl_math::pi ), D );
  76     this->m_DensityCoeff = S( 1 ) / S( std::sqrt( _2piD * double( det ) ) );
  77
  78   } // fi
  79
  80   // Compute minimum and maximum probabilities from input samples
  81   static S sample[ D ];
  82   for( unsigned long i = 0; i < this->m_Samples.size( ); ++i )
  83   {
  84     for( unsigned int d = 0; d < D; ++d )
  85       sample[ d ] = this->m_Samples[ i ][ d ][ 0 ];
  86     S p = this->Probability( sample );
  87     if( i == 0 )
  88     {
  89       this->m_MinimumProbability = p;
  90       this->m_MaximumProbability = p;
  91     }
  92     else
  93     {
  94       if( p < this->m_MinimumProbability ) this->m_MinimumProbability = p;
  95       if( this->m_MaximumProbability < p ) this->m_MaximumProbability = p;
  96
  97     } // fi
  98
  99   } // rof
 100   this->m_Updated = true;
 101
 102   /* DEBUG:
 103      std::cout << "--------------------" << std::endl;
 104      std::cout << this->m_Cov << std::endl;
 105      std::cout << "--------------------" << std::endl;
 106      std::cout << this->m_InvCov << std::endl;
 107      std::cout << "--------------------" << std::endl;
 108      std::cout << this->m_Mean << std::endl;
 109   */
 110 }
 111
 112 // -------------------------------------------------------------------------
 113 template< class S, unsigned int D >
 114 template< class V >
 115 S cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 116 Probability( const V& sample ) const
 117 {
 118   TMatrix c( D, 1 );
 119   for( unsigned int d = 0; d < D; ++d )
 120     c[ d ][ 0 ] = S( sample[ d ] ) - this->m_Mean[ d ][ 0 ];
 121   double v = double( ( c.transpose( ) * ( this->m_InvCov * c ) )[ 0 ][ 0 ] );
 122   v /= double( 2 );
 123
 124   return( S( std::exp( -v ) ) );
 125 }
 126
 127 // -------------------------------------------------------------------------
 128 template< class S, unsigned int D >
 129 S cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 130 Probability( const S& s_x, const S& s_y, ... ) const
 131 {
 132   static S sample[ D ];
 133
 134   std::va_list args_lst;
 135   va_start( args_lst, s_y );
 136   sample[ 0 ] = s_x;
 137   sample[ 1 ] = s_y;
 138   for( unsigned int d = 2; d < D; ++d )
 139     sample[ d ] = S( va_arg( args_lst, double ) );
 140   va_end( args_lst );
 141   return( this->Probability( sample ) );
 142 }
 143
 144 // -------------------------------------------------------------------------
 145 template< class S, unsigned int D >
 146 template< class V, class M >
 147 void
 148 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 149 GetModel( V& m, M& E ) const
 150 {
 151   for( unsigned int i = 0; i < D; ++i )
 152   {
 153     m[ i ] = double( this->m_Mean[ i ][ 0 ] );
 154     for( unsigned int j = 0; j < D; ++j )
 155       E[ i ][ j ] = double( this->m_Cov[ i ][ j ] );
 156
 157   } // rof
 158 }
 159
 160 // -------------------------------------------------------------------------
 161 template< class S, unsigned int D >
 162 void
 163 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 164 Clear( )
 165 {
 166   this->m_Updated = false;
 167   this->m_Samples.clear( );
 168   this->m_S1.set_size( D, 1 );
 169   this->m_S2.set_size( D, D );
 170   this->m_S1.fill( S( 0 ) );
 171   this->m_S2.fill( S( 0 ) );
 172   this->Modified( );
 173 }
 174
 175 // -------------------------------------------------------------------------
 176 template< class S, unsigned int D >
 177 template< class V >
 178 void
 179 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 180 AddSample( const V& sample )
 181 {
 182   TMatrix s( D, 1 );
 183   for( unsigned int d = 0; d < D; ++d )
 184     s[ d ][ 0 ] = S( sample[ d ] );
 185
 186   this->m_Samples.push_back( s );
 187   this->m_S1 += s;
 188   this->m_S2 += s * s.transpose( );
 189
 190   /* DEBUG:
 191      std::cout
 192      << sample[ 0 ] << " " << sample[ 1 ] << " " << sample[ 2 ] << std::endl;
 193   */
 194
 195   /* TODO
 196      {
 197      this->m_S1[ i ][ 0 ] += S( sample[ i ] );
 198      for( unsigned int j = i; j < D; ++j )
 199      {
 200      this->m_S2[ i ][ j ] += S( sample[ i ] ) * S( sample[ j ] );
 201      this->m_S2[ j ][ i ] = this->m_S2[ i ][ j ];
 202
 203      } // rof
 204
 205      } // rof
 206   */
 207   this->m_Updated = false;
 208   this->Modified( );
 209 }
 210
 211 // -------------------------------------------------------------------------
 212 template< class S, unsigned int D >
 213 void
 214 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 215 AddSample( const S& s_x, const S& s_y, ... )
 216 {
 217   static S sample[ D ];
 218
 219   std::va_list args_lst;
 220   va_start( args_lst, s_y );
 221   sample[ 0 ] = s_x;
 222   sample[ 1 ] = s_y;
 223   for( unsigned int d = 2; d < D; ++d )
 224     sample[ d ] = S( va_arg( args_lst, double ) );
 225   va_end( args_lst );
 226   this->AddSample( sample );
 227 }
 228
 229 // -------------------------------------------------------------------------
 230 template< class S, unsigned int D >
 231 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 232 IterativeGaussianModelEstimator( )
 233   : Superclass( )
 234 {
 235   this->Clear( );
 236 }
 237
 238 // -------------------------------------------------------------------------
 239 template< class S, unsigned int D >
 240 cpPlugins::Extensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 241 ~IterativeGaussianModelEstimator( )
 242 {
 243 }
 244
 245 #endif // __CPPLUGINS__EXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
 246
 247 // eof - $RCSfile$