From: Leonardo Florez-Valencia <florez-l@javeriana.edu.co>
Date: Tue, 14 Jun 2016 23:59:55 +0000 (-0500)
Subject: ...
X-Git-Tag: v0.1~143
X-Git-Url: https://git.creatis.insa-lyon.fr/pubgit/?a=commitdiff_plain;h=b3bb78df1a899c796586df5c0a6eec8c84f9b739;p=cpPlugins.git

...
---

diff --git a/appli/examples/extensions/CMakeLists.txt b/appli/examples/extensions/CMakeLists.txt
index b39f42c..26f7d50 100644
--- a/appli/examples/extensions/CMakeLists.txt
+++ b/appli/examples/extensions/CMakeLists.txt
@@ -1,5 +1,6 @@
 SET(
   examples_SOURCES
+  example_IterativeGaussianModelEstimator
   example_KalmanVelocity
   example_ImageSlice
   )
diff --git a/appli/examples/extensions/example_IterativeGaussianModelEstimator.cxx b/appli/examples/extensions/example_IterativeGaussianModelEstimator.cxx
new file mode 100644
index 0000000..b168dcb
--- /dev/null
+++ b/appli/examples/extensions/example_IterativeGaussianModelEstimator.cxx
@@ -0,0 +1,81 @@
+#include <cmath>
+#include <iostream>
+#include <random>
+#include <vector>
+
+#include <cpExtensions/Algorithms/IterativeGaussianModelEstimator.h>
+
+// -------------------------------------------------------------------------
+const unsigned int Dim = 1;
+typedef double TScalar;
+typedef
+cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< TScalar, Dim > TEstimator;
+
+// -------------------------------------------------------------------------
+int main( int argc, char* argv[] )
+{
+  if( argc < 4 )
+  {
+    std::cerr << "Usage: " << argv[ 0 ] << " mean std samples" << std::endl;
+    return( 1 );
+
+  } // fi
+  TScalar mean = std::atof( argv[ 1 ] );
+  TScalar var = std::atof( argv[ 2 ] );
+  unsigned int samples = std::atoi( argv[ 3 ] );
+  var *= var;
+
+  // Prepare estimator
+  TEstimator::Pointer estimator = TEstimator::New( );
+ 
+  // Generate numbers
+  std::random_device r;
+  std::seed_seq seed{ r( ), r( ), r( ), r( ), r( ), r( ), r( ), r( ) };
+  std::mt19937 e( seed );
+  std::normal_distribution< > dist( mean, std::sqrt( var ) );
+  double local_mean = double( 0 );
+  std::vector< double > data;
+  for( unsigned int s = 0; s < samples; ++s )
+  {
+    double v = dist( e );
+    estimator->AddSample( v );
+    local_mean += v;
+    data.push_back( v );
+
+  } // rof
+  local_mean /= double( samples );
+
+  double local_var = double( 0 );
+  for( auto d = data.begin( ); d != data.end( ); ++d )
+    local_var += ( *d - local_mean ) * ( *d - local_mean );
+  local_var /= double( samples - 1 );
+  
+  // Show results
+  std::cout
+    << "Mean: "
+    << mean << " <-> "
+    << estimator->GetMean( )[ 0 ] << " <-> "
+    << local_mean
+    << std::endl;
+  std::cout
+    << "Var: "
+    << var << " <-> "
+    << estimator->GetCovariance( )[ 0 ][ 0 ] << " <-> "
+    << estimator->GetUnbiasedCovariance( )[ 0 ][ 0 ] << " <-> "
+    << local_var
+    << std::endl;
+
+  std::cout << "--------------------------------------" << std::endl;
+  for( unsigned int s = 0; s < 15; ++s )
+  {
+    double v = dist( e );
+    double d = std::sqrt( estimator->SquaredMahalanobis( v ) );
+    std::cout << "Distante to " << v << " is " << d << std::endl;
+
+  } // rof
+
+  return( 0 );
+}
+
+// eof - $RCSfile$
diff --git a/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.h b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.h
index 5cf7332..95947cf 100644
--- a/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.h
+++ b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.h
@@ -8,15 +8,22 @@
 #include <vector>
 #include <itkConceptChecking.h>
 #include <itkObject.h>
-#include <vnl/vnl_matrix.h>
+#include <itkObjectFactory.h>
+
+#include <itkCovariantVector.h>
+#include <itkMatrix.h>
+#include <itkPoint.h>
+#include <itkVector.h>
 
 namespace cpExtensions
 {
   namespace Algorithms
   {
     /**
+     * Recursive formulation taken from:
+     * http://lmb.informatik.uni-freiburg.de/lectures/mustererkennung/Englische_Folien/07_c_ME_en.pdf
      */
-    template< class S, unsigned int D >
+    template< class _TScalar, unsigned int _VDimension >
     class IterativeGaussianModelEstimator
       : public itk::Object
     {
@@ -26,61 +33,90 @@ namespace cpExtensions
       typedef itk::SmartPointer< Self >       Pointer;
       typedef itk::SmartPointer< const Self > ConstPointer;
 
-      typedef S TScalar;
-      itkStaticConstMacro( Dimension, unsigned int, D );
+      typedef _TScalar TScalar;
+      itkStaticConstMacro( Dimension, unsigned int, _VDimension );
 
       // Begin concept checking
 #ifdef ITK_USE_CONCEPT_CHECKING
       itkConceptMacro(
         ScalarTypeHasFloatResolution,
-        ( itk::Concept::IsFloatingPoint< S > )
+        ( itk::Concept::IsFloatingPoint< _TScalar > )
         );
 #endif
       // End concept checking
 
-      typedef vnl_matrix< S >        TMatrix;
-      typedef std::vector< TMatrix > TMatrices;
+      typedef itk::Matrix< TScalar, Dimension, Dimension > TMatrix;
+      typedef itk::Vector< TScalar, Dimension > TVector;
 
     public:
       itkNewMacro( Self );
       itkTypeMacro( IterativeGaussianModelEstimator, itkObject );
 
+      itkGetConstMacro( Mean, TVector );
+      itkGetConstMacro( Covariance, TMatrix );
+      itkGetConstMacro( UnbiasedCovariance, TMatrix );
+      itkGetConstMacro( NumberOfSamples, unsigned long );
+
+      itkSetMacro( Mean, TVector );
+      itkSetMacro( Covariance, TMatrix );
+      itkSetMacro( NumberOfSamples, unsigned long );
+
     public:
-      unsigned long GetNumberOfSamples( ) const
-        { return( ( unsigned long )( this->m_N ) ); }
-      const TMatrix& GetMu( ) const
-        { return( this->m_M ); }
-      const TMatrix& GetOmega( ) const
-        { return( this->m_O ); }
+      void Clear( );
+
+      template< class _TOtherScalar >
+      void AddSample( const _TOtherScalar& x1, ... );
 
-      void SetNumberOfSamples( unsigned long n );
-      void SetMu( const TMatrix& m );
-      void SetOmega( const TMatrix& O );
+      template< class _TOtherScalar >
+      void AddSample( const _TOtherScalar* array );
 
-      bool SaveModelToFile( const std::string& filename ) const;
-      bool LoadModelFromFile( const std::string& filename );
+      template< class _TOtherScalar >
+      void AddSample( const vnl_vector< _TOtherScalar >& v );
 
-      template< class V >
-      S Probability( const V& sample ) const;
+      template< class _TOtherScalar >
+      void AddSample(
+        const itk::CovariantVector< _TOtherScalar, _VDimension >& v
+        );
 
-      S Probability( const S& s_x, const S& s_y, ... ) const;
+      template< class _TOtherScalar >
+      void AddSample( const itk::Point< _TOtherScalar, _VDimension >& p );
 
-      template< class V, class M >
-      void GetModel( V& m, M& E ) const;
+      template< class _TOtherScalar >
+      void AddSample( const itk::Vector< _TOtherScalar, _VDimension >& v );
 
-      void Clear( );
+      template< class _TOtherScalar >
+      _TScalar SquaredMahalanobis( const _TOtherScalar& x1, ... ) const;
+
+      template< class _TOtherScalar >
+      _TScalar SquaredMahalanobis( const _TOtherScalar* array ) const;
+
+      template< class _TOtherScalar >
+      _TScalar SquaredMahalanobis(
+        const vnl_vector< _TOtherScalar >& v
+        ) const;
+
+      template< class _TOtherScalar >
+      _TScalar SquaredMahalanobis(
+        const itk::CovariantVector< _TOtherScalar, _VDimension >& v
+        ) const;
 
-      template< class V >
-      void AddSample( const V& sample );
+      template< class _TOtherScalar >
+      _TScalar SquaredMahalanobis(
+        const itk::Point< _TOtherScalar, _VDimension >& p
+        ) const;
 
-      void AddSample( const S& s_x, const S& s_y, ... );
+      template< class _TOtherScalar >
+      _TScalar SquaredMahalanobis(
+        const itk::Vector< _TOtherScalar, _VDimension >& v
+        ) const;
 
     protected:
       IterativeGaussianModelEstimator( );
       virtual ~IterativeGaussianModelEstimator( );
 
     protected:
-      void _UpdateModel( ) const;
+      void _AddSample( const TVector& v ) const;
+      _TScalar _SquaredMahalanobis( const TVector& v ) const;
 
     private:
       // Purposely not implemented
@@ -88,14 +124,13 @@ namespace cpExtensions
       void operator=( const Self& other );
 
     protected:
-      S m_N;
-      TMatrix m_M;
-      TMatrix m_O;
-
-      mutable bool m_Updated;
-      mutable TMatrix m_Cov;
-      mutable TMatrix m_Inv;
-      mutable S m_Norm;
+      // Recursive avg/cov values
+      mutable unsigned long m_NumberOfSamples;
+      mutable TVector m_Mean;
+      mutable TMatrix m_Covariance;
+      mutable TMatrix m_UnbiasedCovariance;
+      mutable bool m_InverseUpdated;
+      mutable TMatrix m_InverseUnbiasedCovariance;
     };
 
   } // ecapseman
diff --git a/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
index c6e3cb8..a297c3c 100644
--- a/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
+++ b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
@@ -5,239 +5,193 @@
 #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( );
-
-  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 );
-
-    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 ) );
-
-  } // fi
+  TVector s;
+  for( unsigned d = 0; d < _VDimension; ++d )
+    s[ d ] = TScalar( p[ d ] );
+  this->_AddSample( 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 >
+void cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
+AddSample( const itk::Vector< _TOtherScalar, _VDimension >& v )
 {
-  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 ) );
+  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, 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 _TOtherScalar& x1, ... ) 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 ] );
-
-  } // rof
+  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 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 _TOtherScalar* array ) 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( array[ 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 >::
+SquaredMahalanobis( const vnl_vector< _TOtherScalar >& v ) 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;
-
-  } // fi
-
-  this->m_Updated = false;
-  this->Modified( );
+  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 >
-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 >::
+SquaredMahalanobis(
+  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->_SquaredMahalanobis( 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 >::
+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 ) );
+}
 
-  } // fi
-  this->AddSample( sample );
+// -------------------------------------------------------------------------
+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 S, unsigned int D >
-cpExtensions::Algorithms::IterativeGaussianModelEstimator< S, D >::
+template< class _TScalar, unsigned int _VDimension >
+cpExtensions::Algorithms::
+IterativeGaussianModelEstimator< _TScalar, _VDimension >::
 IterativeGaussianModelEstimator( )
   : Superclass( )
 {
@@ -245,45 +199,56 @@ 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 );
+  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 ];
 
-  // Update covariance
-  this->m_Cov =
-    this->m_O -
-    (
-      ( this->m_M * this->m_M.transpose( ) ) *
-      ( this->m_N / ( this->m_N - S( 1 ) ) )
-      );
+  } // 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;
+}
 
-  // Compute inverse and determinant
-  S det = vnl_determinant( this->m_Cov );
-  if( !( det > S( 0 ) ) )
-  {
-    this->m_Inv.set_size( D, D );
-    this->m_Inv.fill( S( 0 ) );
-    this->m_Norm = S( 0 );
-  }
-  else
+// -------------------------------------------------------------------------
+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 ) ) );
+    this->m_InverseUnbiasedCovariance =
+      this->m_UnbiasedCovariance.GetInverse( );
+    this->m_InverseUpdated = true;
 
   } // fi
-
-  // Object now is updated
-  this->m_Updated = true;
+  TVector x = v - this->m_Mean;
+  return( x * ( this->m_InverseUnbiasedCovariance * x ) );
 }
 
 #endif // __CPEXTENSIONS__ALGORITHMS__ITERATIVEGAUSSIANMODELESTIMATOR__HXX__