...

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

diff --git a/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
index a297c3cad08bbeb33de78329dd7480bb447f59f9..acd1da76f82f380381eac997b8aed548012c4013 100644 (file)
--- a/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
+++ b/lib/cpExtensions/Algorithms/IterativeGaussianModelEstimator.hxx
@@ -188,6 +188,91 @@ SquaredMahalanobis( const itk::Vector< _TOtherScalar, _VDimension >& v ) const
   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::
@@ -242,8 +327,15 @@ _SquaredMahalanobis( const TVector& v ) const
 {
   if( !this->m_InverseUpdated )
   {
-    this->m_InverseUnbiasedCovariance =
-      this->m_UnbiasedCovariance.GetInverse( );
+    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
@@ -251,6 +343,23 @@ _SquaredMahalanobis( const TVector& v ) const
   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$