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