yet another refactoring

[cpPlugins.git] / lib / cpExtensions / Algorithms / IterativeGaussianModelEstimator.hxx

diff --git a/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
new file mode 100644 (file)
index 0000000..acd1da7
--- /dev/null
+++ b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
@@ -0,0 +1,365 @@
+// -------------------------------------------------------------------------
+// @author Leonardo Florez-Valencia (florez-l@javeriana.edu.co)
+// -------------------------------------------------------------------------
+
+#ifndef __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
+#define __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__
+
+#include <cstdarg>
+
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Clear( )
+{
+  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 _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const _TOtherScalar& x1, ... )
+{
+  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 _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const _TOtherScalar* array )
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( array[ d ] );
+  this->_AddSample( s );
+}
+
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const vnl_vector< _TOtherScalar >& v )
+{
+  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 _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const itk::CovariantVector< _TOtherScalar, _VDimension >& v )
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  this->_AddSample( s );
+}
+
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const itk::Point< _TOtherScalar, _VDimension >& p )
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( p[ d ] );
+  this->_AddSample( s );
+}
+
+// -------------------------------------------------------------------------
+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 );
+}
+
+// -------------------------------------------------------------------------
+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 ) );
+}
+
+// -------------------------------------------------------------------------
+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 _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+SquaredMahalanobis( 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->_SquaredMahalanobis( s ) );
+}
+
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+SquaredMahalanobis(
+  const itk::CovariantVector< _TOtherScalar, _VDimension >& v
+  ) const
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  return( this->_SquaredMahalanobis( s ) );
+}
+
+// -------------------------------------------------------------------------
+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 _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+SquaredMahalanobis( const itk::Vector< _TOtherScalar, _VDimension >& v ) const
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  return( this->_SquaredMahalanobis( s ) );
+}
+
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Density( 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->_Density( s ) );
+}
+
+// -------------------------------------------------------------------------
+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 ) );
+}
+
+// -------------------------------------------------------------------------
+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 _TScalar, unsigned int _VDimension >
+template< class _TOtherScalar >
+_TScalar cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+Density(
+  const itk::CovariantVector< _TOtherScalar, _VDimension >& v
+  ) const
+{
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( v[ d ] );
+  return( this->_Density( s ) );
+}
+
+// -------------------------------------------------------------------------
+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 ) );
+}
+
+// -------------------------------------------------------------------------
+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 _TScalar, unsigned int _VDimension >
+cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+IterativeGaussianModelEstimator( )
+  : Superclass( )
+{
+  this->Clear( );
+}
+
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+~IterativeGaussianModelEstimator( )
+{
+}
+
+// -------------------------------------------------------------------------
+template< class _TScalar, unsigned int _VDimension >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+_AddSample( const TVector& v ) const
+{
+  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 )
+  {
+    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 )
+  {
+    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 ) );
+}
+
+// -------------------------------------------------------------------------
+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__
+
+// eof - $RCSfile$