lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx

   1 // -------------------------------------------------------------------------
   2 // @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
   3 // -------------------------------------------------------------------------
   4
   5 #ifndef __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
   6 #define __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
   7
   8 #include <cmath>
   9 #include <cstdarg>
  10 #include <fstream>
  11 #include <vnl/vnl_math.h>
  12 #include <vnl/vnl_inverse.h>
  13
  14 // -------------------------------------------------------------------------
  15 template< class S, unsigned int D >
  16 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
  17 SetNumberOfSamples( unsigned long n )
  18 {
  19   this->m_N = S( n );
  20   this->m_Updated = false;
  21   this->Modified( );
  22 }
  23
  24 // -------------------------------------------------------------------------
  25 template< class S, unsigned int D >
  26 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
  27 SetMu( const TMatrix& m )
  28 {
  29   if( m.rows( ) == D && m.columns( ) == 1 )
  30   {
  31     this->m_M = m;
  32     this->m_Updated = false;
  33     this->Modified( );
  34   }
  35   else
  36   {
  37     itkExceptionMacro(
  38       << "Input Mu matrix is not a " << D << "x1 matrix"
  39       );
  40
  41   } // fi
  42 }
  43
  44 // -------------------------------------------------------------------------
  45 template< class S, unsigned int D >
  46 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
  47 SetOmega( const TMatrix& O )
  48 {
  49   if( O.rows( ) == D && O.columns( ) == D )
  50   {
  51     this->m_O = O;
  52     this->m_Updated = false;
  53     this->Modified( );
  54   }
  55   else
  56   {
  57     itkExceptionMacro(
  58       << "Input Omega matrix is not a " << D << "x" << D << " matrix"
  59       );
  60
  61   } // fi
  62 }
  63
  64 // -------------------------------------------------------------------------
  65 template< class S, unsigned int D >
  66 bool cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
  67 SaveModelToFile( const std::string& filename ) const
  68 {
  69   std::ofstream out( filename.c_str( ) );
  70   if( out )
  71   {
  72     out << this->m_N << std::endl;
  73     out << this->m_M << std::endl;
  74     out << this->m_O << std::endl;
  75     out.close( );
  76     return( true );
  77   }
  78   else
  79     return( false );
  80 }
  81
  82 // -------------------------------------------------------------------------
  83 template< class S, unsigned int D >
  84 bool cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
  85 LoadModelFromFile( const std::string& filename )
  86 {
  87   std::ifstream in( filename.c_str( ) );
  88   if( in )
  89   {
  90     this->Clear( );
  91     in >> this->m_N;
  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 ];
  97     in.close( );
  98     return( true );
  99   }
 100   else
 101     return( false );
 102 }
 103
 104 // -------------------------------------------------------------------------
 105 template< class S, unsigned int D >
 106 template< class V >
 107 S cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 108 Probability( const V& sample ) const
 109 {
 110   if( !this->m_Updated )
 111     this->_UpdateModel( );
 112
 113   TMatrix c( D, 1 );
 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 )
 117   {
 118     // Covariance is NOT null
 119     double v = double( ( c.transpose( ) * ( this->m_Inv * c ) )[ 0 ][ 0 ] );
 120     v /= double( 2 );
 121
 122     return( S( std::exp( -v ) ) );
 123   }
 124   else
 125   {
 126     // Covariance is null
 127     S n = S( 0 );
 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 ) );
 132
 133   } // fi
 134 }
 135
 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
 140 {
 141   static S sample[ D ];
 142
 143   std::va_list args_lst;
 144   va_start( args_lst, s_y );
 145   sample[ 0 ] = s_x;
 146   sample[ 1 ] = s_y;
 147   for( unsigned int d = 2; d < D; ++d )
 148     sample[ d ] = S( va_arg( args_lst, double ) );
 149   va_end( args_lst );
 150   return( this->Probability( sample ) );
 151 }
 152
 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
 158 {
 159   if( !this->m_Updated )
 160     this->_UpdateModel( );
 161   for( unsigned int i = 0; i < D; ++i )
 162   {
 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 ] );
 166
 167   } // rof
 168 }
 169
 170 // -------------------------------------------------------------------------
 171 template< class S, unsigned int D >
 172 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 173 Clear( )
 174 {
 175   this->m_N = S( 0 );
 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;
 181   this->Modified( );
 182 }
 183
 184 // -------------------------------------------------------------------------
 185 template< class S, unsigned int D >
 186 template< class V >
 187 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 188 AddSample( const V& sample )
 189 {
 190   TMatrix s( D, 1 );
 191   for( unsigned int d = 0; d < D; ++d )
 192     s[ d ][ 0 ] = S( sample[ d ] );
 193
 194   // Update mean
 195   S coeff = this->m_N;
 196   this->m_N += S( 1 );
 197   coeff /= this->m_N;
 198   this->m_M = ( this->m_M * coeff ) + ( s / this->m_N );
 199
 200   // Update omega operand
 201   if( this->m_N == 1 )
 202     this->m_O  = s * s.transpose( );
 203   else if( this->m_N == 2 )
 204     this->m_O += s * s.transpose( );
 205   else
 206   {
 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;
 210
 211   } // fi
 212
 213   this->m_Updated = false;
 214   this->Modified( );
 215 }
 216
 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, ... )
 221 {
 222   static S sample[ D ];
 223
 224   std::va_list args_lst;
 225   va_start( args_lst, s_y );
 226   sample[ 0 ] = s_x;
 227   if( D > 1 )
 228   {
 229     sample[ 1 ] = s_y;
 230     for( unsigned int d = 2; d < D; ++d )
 231       sample[ d ] = S( va_arg( args_lst, double ) );
 232     va_end( args_lst );
 233
 234   } // fi
 235   this->AddSample( sample );
 236 }
 237
 238 // -------------------------------------------------------------------------
 239 template< class S, unsigned int D >
 240 cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 241 IterativeGaussianModelEstimator( )
 242   : Superclass( )
 243 {
 244   this->Clear( );
 245 }
 246
 247 // -------------------------------------------------------------------------
 248 template< class S, unsigned int D >
 249 cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 250 ~IterativeGaussianModelEstimator( )
 251 {
 252 }
 253
 254 // -------------------------------------------------------------------------
 255 template< class S, unsigned int D >
 256 void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
 257 _UpdateModel( ) const
 258 {
 259   static const double _2piD =
 260     std::pow( double( 2 ) * double( vnl_math::pi ), D );
 261
 262   // Update covariance
 263   this->m_Cov =
 264     this->m_O -
 265     (
 266       ( this->m_M * this->m_M.transpose( ) ) *
 267       ( this->m_N / ( this->m_N - S( 1 ) ) )
 268       );
 269
 270   // Compute inverse and determinant
 271   S det = vnl_determinant( this->m_Cov );
 272   if( !( det > S( 0 ) ) )
 273   {
 274     this->m_Inv.set_size( D, D );
 275     this->m_Inv.fill( S( 0 ) );
 276     this->m_Norm = S( 0 );
 277   }
 278   else
 279   {
 280     this->m_Inv = vnl_inverse( this->m_Cov );
 281     this->m_Norm = S( 1 ) / S( std::sqrt( _2piD * double( det ) ) );
 282
 283   } // fi
 284
 285   // Object now is updated
 286   this->m_Updated = true;
 287 }
 288
 289 #endif // __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
 290
 291 // eof - $RCSfile$