X-Git-Url: https://git.creatis.insa-lyon.fr/pubgit/?a=blobdiff_plain;f=lib%2FcpExtensions%2FAlgorithms%2FIterativeGaussianModelEstimator.hxx;h=acd1da76f82f380381eac997b8aed548012c4013;hb=aee3cafa7e93f996580777976636ed625dbc43f5;hp=c6e3cb8fdc6fd444a725528114cc1eedcea94624;hpb=1b0022070ff3b5f80f6f8c8b87f73032f5685eaf;p=cpPlugins.git

diff --git a/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
index c6e3cb8..acd1da7 100644
--- a/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
+++ b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
@@ -5,239 +5,278 @@
 #ifndef __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
 #define __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
 
-#include <cmath>
 #include <cstdarg>
-#include <fstream>
-#include <vnl/vnl_math.h>
-#include <vnl/vnl_inverse.h>
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-SetNumberOfSamples( unsigned long n )
+template< class _TScalar, unsigned int _VDimension >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Clear( )
 {
-  this->m_N = S( n );
-  this->m_Updated = false;
-  this->Modified( );
+  this->m_NumberOfSamples = 0;
+  this->m_Mean.Fill( TScalar( 0 ) );
+  this->m_Covariance.Fill( TScalar( 0 ) );
+  this->m_InverseUpdated = false;
+  this->m_InverseUnbiasedCovariance.Fill( TScalar( 0 ) );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-SetMu( const TMatrix& m )
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const _TOtherScalar& x1, ... )
 {
-  if( m.rows( ) == D && m.columns( ) == 1 )
-  {
-    this->m_M = m;
-    this->m_Updated = false;
-    this->Modified( );
-  }
-  else
-  {
-    itkExceptionMacro(
-      << "Input Mu matrix is not a " << D << "x1 matrix"
-      );
-
-  } // fi
+  TVector s;
+  std::va_list lst;
+  va_start( lst, x1 );
+  s[ 0 ] = TScalar( x1 );
+  for( unsigned int d = 1; d < _VDimension; ++d )
+    s[ d ] = TScalar( va_arg( lst, double ) );
+  va_end( lst );
+  this->_AddSample( s );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-SetOmega( const TMatrix& O )
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const _TOtherScalar* array )
 {
-  if( O.rows( ) == D && O.columns( ) == D )
-  {
-    this->m_O = O;
-    this->m_Updated = false;
-    this->Modified( );
-  }
-  else
-  {
-    itkExceptionMacro(
-      << "Input Omega matrix is not a " << D << "x" << D << " matrix"
-      );
-
-  } // fi
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( array[ d ] );
+  this->_AddSample( s );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-bool cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-SaveModelToFile( const std::string& filename ) const
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const vnl_vector< _TOtherScalar >& v )
 {
-  std::ofstream out( filename.c_str( ) );
-  if( out )
-  {
-    out << this->m_N << std::endl;
-    out << this->m_M << std::endl;
-    out << this->m_O << std::endl;
-    out.close( );
-    return( true );
-  }
-  else
-    return( false );
+  unsigned int len = ( v.size( ) < _VDimension )? v.size: _VDimension;
+  TVector s;
+  for( unsigned d = 0; d < len; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  this->_AddSample( s );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-bool cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-LoadModelFromFile( const std::string& filename )
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const itk::CovariantVector< _TOtherScalar, _VDimension >& v )
 {
-  std::ifstream in( filename.c_str( ) );
-  if( in )
-  {
-    this->Clear( );
-    in >> this->m_N;
-    for( unsigned int i = 0; i < D; ++i )
-      in >> this->m_M[ i ][ 0 ];
-    for( unsigned int j = 0; j < D; ++j )
-      for( unsigned int i = 0; i < D; ++i )
-        in >> this->m_O[ i ][ j ];
-    in.close( );
-    return( true );
-  }
-  else
-    return( false );
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  this->_AddSample( s );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-template< class V >
-S cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-Probability( const V& sample ) const
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const itk::Point< _TOtherScalar, _VDimension >& p )
 {
-  if( !this->m_Updated )
-    this->_UpdateModel( );
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( p[ d ] );
+  this->_AddSample( s );
+}
 
-  TMatrix c( D, 1 );
-  for( unsigned int d = 0; d < D; ++d )
-    c[ d ][ 0 ] = S( sample[ d ] ) - this->m_M[ d ][ 0 ];
-  if( S( 0 ) < this->m_Norm )
-  {
-    // Covariance is NOT null
-    double v = double( ( c.transpose( ) * ( this->m_Inv * c ) )[ 0 ][ 0 ] );
-    v /= double( 2 );
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const itk::Vector< _TOtherScalar, _VDimension >& v )
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  this->_AddSample( s );
+}
 
-    return( S( std::exp( -v ) ) );
-  }
-  else
-  {
-    // Covariance is null
-    S n = S( 0 );
-    for( unsigned int d = 0; d < D; ++d )
-      n += c[ d ][ 0 ] * c[ d ][ 0 ];
-    bool equal = ( double( n ) < double( 1e-10 ) );
-    return( ( equal )? S( 1 ): S( 0 ) );
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+SquaredMahalanobis( const _TOtherScalar& x1, ... ) const
+{
+  TVector s;
+  std::va_list lst;
+  va_start( lst, x1 );
+  s[ 0 ] = TScalar( x1 );
+  for( unsigned int d = 1; d < _VDimension; ++d )
+    s[ d ] = TScalar( va_arg( lst, double ) );
+  va_end( lst );
+  return( this->_SquaredMahalanobis( s ) );
+}
 
-  } // fi
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+SquaredMahalanobis( const _TOtherScalar* array ) const
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( array[ d ] );
+  return( this->_SquaredMahalanobis( s ) );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-S cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-Probability( const S& s_x, const S& s_y, ... ) const
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+SquaredMahalanobis( const vnl_vector< _TOtherScalar >& v ) 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 ) );
+  unsigned int len = ( v.size( ) < _VDimension )? v.size: _VDimension;
+  TVector s;
+  for( unsigned d = 0; d < len; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  return( this->_SquaredMahalanobis( s ) );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-template< class V, class M >
-void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-GetModel( V& m, M& E ) const
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+SquaredMahalanobis(
+  const itk::CovariantVector< _TOtherScalar, _VDimension >& v
+  ) const
 {
-  if( !this->m_Updated )
-    this->_UpdateModel( );
-  for( unsigned int i = 0; i < D; ++i )
-  {
-    m[ i ] = double( this->m_M[ i ][ 0 ] );
-    for( unsigned int j = 0; j < D; ++j )
-      E[ i ][ j ] = double( this->m_Cov[ i ][ j ] );
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  return( this->_SquaredMahalanobis( s ) );
+}
 
-  } // rof
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+SquaredMahalanobis( const itk::Point< _TOtherScalar, _VDimension >& p ) const
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( p[ d ] );
+  return( this->_SquaredMahalanobis( s ) );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-Clear( )
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+SquaredMahalanobis( const itk::Vector< _TOtherScalar, _VDimension >& v ) const
 {
-  this->m_N = S( 0 );
-  this->m_M.set_size( D, 1 );
-  this->m_O.set_size( D, D );
-  this->m_M.fill( S( 0 ) );
-  this->m_O.fill( S( 0 ) );
-  this->m_Updated = false;
-  this->Modified( );
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  return( this->_SquaredMahalanobis( s ) );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-template< class V >
-void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-AddSample( const V& sample )
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Density( const _TOtherScalar& x1, ... ) const
 {
-  TMatrix s( D, 1 );
-  for( unsigned int d = 0; d < D; ++d )
-    s[ d ][ 0 ] = S( sample[ d ] );
-
-  // Update mean
-  S coeff = this->m_N;
-  this->m_N += S( 1 );
-  coeff /= this->m_N;
-  this->m_M = ( this->m_M * coeff ) + ( s / this->m_N );
-
-  // Update omega operand
-  if( this->m_N == 1 )
-    this->m_O  = s * s.transpose( );
-  else if( this->m_N == 2 )
-    this->m_O += s * s.transpose( );
-  else
-  {
-    S inv = S( 1 ) / ( this->m_N - S( 1 ) );
-    this->m_O  = this->m_O * ( this->m_N - S( 2 ) ) * inv;
-    this->m_O += ( s * s.transpose( ) ) * inv;
+  TVector s;
+  std::va_list lst;
+  va_start( lst, x1 );
+  s[ 0 ] = TScalar( x1 );
+  for( unsigned int d = 1; d < _VDimension; ++d )
+    s[ d ] = TScalar( va_arg( lst, double ) );
+  va_end( lst );
+  return( this->_Density( s ) );
+}
 
-  } // fi
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Density( const _TOtherScalar* array ) const
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( array[ d ] );
+  return( this->_Density( s ) );
+}
 
-  this->m_Updated = false;
-  this->Modified( );
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Density( const vnl_vector< _TOtherScalar >& v ) const
+{
+  unsigned int len = ( v.size( ) < _VDimension )? v.size: _VDimension;
+  TVector s;
+  for( unsigned d = 0; d < len; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  return( this->_Density( s ) );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-AddSample( const S& s_x, const S& s_y, ... )
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Density(
+  const itk::CovariantVector< _TOtherScalar, _VDimension >& v
+  ) const
 {
-  static S sample[ D ];
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  return( this->_Density( s ) );
+}
 
-  std::va_list args_lst;
-  va_start( args_lst, s_y );
-  sample[ 0 ] = s_x;
-  if( D > 1 )
-  {
-    sample[ 1 ] = s_y;
-    for( unsigned int d = 2; d < D; ++d )
-      sample[ d ] = S( va_arg( args_lst, double ) );
-    va_end( args_lst );
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Density( const itk::Point< _TOtherScalar, _VDimension >& p ) const
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( p[ d ] );
+  return( this->_Density( s ) );
+}
 
-  } // fi
-  this->AddSample( sample );
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Density( const itk::Vector< _TOtherScalar, _VDimension >& v ) const
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  return( this->_Density( s ) );
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
+template< class _TScalar, unsigned int _VDimension >
+cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
 IterativeGaussianModelEstimator( )
   : Superclass( )
 {
@@ -245,45 +284,80 @@ IterativeGaussianModelEstimator( )
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
+template< class _TScalar, unsigned int _VDimension >
+cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
 ~IterativeGaussianModelEstimator( )
 {
 }
 
 // -------------------------------------------------------------------------
-template< class S, unsigned int D >
-void cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
-_UpdateModel( ) const
+template< class _TScalar, unsigned int _VDimension >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+_AddSample( const TVector& v ) const
 {
-  static const double _2piD =
-    std::pow( double( 2 ) * double( vnl_math::pi ), D );
-
-  // Update covariance
-  this->m_Cov =
-    this->m_O -
-    (
-      ( this->m_M * this->m_M.transpose( ) ) *
-      ( this->m_N / ( this->m_N - S( 1 ) ) )
-      );
-
-  // Compute inverse and determinant
-  S det = vnl_determinant( this->m_Cov );
-  if( !( det > S( 0 ) ) )
+  this->m_NumberOfSamples += 1;
+  TScalar a = TScalar( 1 ) / TScalar( this->m_NumberOfSamples );
+  TScalar b = TScalar( 1 ) - a;
+  TVector c = v - this->m_Mean;
+  TMatrix m;
+  for( unsigned int i = 0; i < _VDimension; ++i )
   {
-    this->m_Inv.set_size( D, D );
-    this->m_Inv.fill( S( 0 ) );
-    this->m_Norm = S( 0 );
-  }
-  else
+    m[ i ][ i ] = c[ i ] * c[ i ];
+    for( unsigned int j = i + 1; j < _VDimension; ++j )
+      m[ i ][ j ] = m[ j ][ i ] = c[ i ] * c[ j ];
+
+  } // rof
+  this->m_Covariance = ( this->m_Covariance + ( m * a ) ) * b;
+  this->m_Mean = ( this->m_Mean * b ) + ( v * a );
+
+  TScalar u =
+    TScalar( this->m_NumberOfSamples ) /
+    TScalar( this->m_NumberOfSamples - 1 );
+  this->m_UnbiasedCovariance = this->m_Covariance * u;
+  this->m_InverseUpdated = false;
+}
+
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+_SquaredMahalanobis( const TVector& v ) const
+{
+  if( !this->m_InverseUpdated )
   {
-    this->m_Inv = vnl_inverse( this->m_Cov );
-    this->m_Norm = S( 1 ) / S( std::sqrt( _2piD * double( det ) ) );
+    double d =
+      double(
+        vnl_determinant( this->m_UnbiasedCovariance.GetVnlMatrix( ) )
+        );
+    if( std::fabs( d ) > double( 0 ) )
+      this->m_InverseUnbiasedCovariance =
+        this->m_UnbiasedCovariance.GetInverse( );
+    else
+      this->m_InverseUnbiasedCovariance.SetIdentity( );
+    this->m_InverseUpdated = true;
 
   } // fi
+  TVector x = v - this->m_Mean;
+  return( x * ( this->m_InverseUnbiasedCovariance * x ) );
+}
 
-  // Object now is updated
-  this->m_Updated = true;
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+_Density( const TVector& v ) const
+{
+  static const double _2pik =
+    std::pow( double( 2 ) * double( vnl_math::pi ), _VDimension );
+
+  double s = double( this->_SquaredMahalanobis( v ) ) / double( 2 );
+  double d =
+    double(
+      vnl_determinant( this->m_UnbiasedCovariance.GetVnlMatrix( ) )
+      );
+  return( _TScalar( std::exp( -s ) / std::sqrt( _2pik * d ) ) );
 }
 
 #endif // __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__