diff --git a/Code/Common/otbHistogramStatisticsFunction.txx b/Code/Common/otbHistogramStatisticsFunction.txx
index 760b9539e1ef0558d8f7d2346e4b9f59564f2c72..9db1e18037d3e6176fc5275b0732694e277b7eb9 100644
--- a/Code/Common/otbHistogramStatisticsFunction.txx
+++ b/Code/Common/otbHistogramStatisticsFunction.txx
@@ -106,10 +106,20 @@ HistogramStatisticsFunction< TInputHistogram, TOutput>
 {
   typename TInputHistogram::ConstPointer histogram = m_InputHistogram;
 
+
   unsigned int NumberOfDimension =  histogram->GetSize().GetSizeDimension();
-  std::vector<TOutput> GlobalMean(NumberOfDimension);
   m_mean.resize(NumberOfDimension);
 
+  if(histogram->GetTotalFrequency() == 0)
+    {
+    itkExceptionMacro(<<"Histogram must contain at least 1 element.");
+    }
+
+  if(NumberOfDimension > 2)
+    {
+    itkExceptionMacro(<<"Histogram must have 1 or 2 dimension.");
+    }
+
   for( unsigned int noDim = 0; noDim < NumberOfDimension; noDim++ )
     {
     MeasurementType mean = itk::NumericTraits<MeasurementType>::Zero;
@@ -129,37 +139,38 @@ void
 HistogramStatisticsFunction< TInputHistogram, TOutput>
 ::CalculateCovariance()
 {
-#if 0
-  typename TInputHistogram::ConstPointer histogram = m_InputHistogram;
+  CalculateMean();
 
-  // TODO: as an improvement, the class could accept multi-dimensional histograms
-  // and the user could specify the dimension to apply the algorithm to.
-  if (histogram->GetSize().GetSizeDimension() != 1)
-    {
-    itkExceptionMacro(<<"Histogram must be 1-dimensional.");
-    }
+  typename TInputHistogram::ConstPointer histogram = m_InputHistogram;
 
-  // compute global mean
-  typename TInputHistogram::ConstIterator iter = histogram->Begin() ;
-  typename TInputHistogram::ConstIterator end = histogram->End() ;
+  unsigned int NumberOfDimension =  histogram->GetSize().GetSizeDimension();
+  m_covariance.resize(NumberOfDimension*NumberOfDimension);
 
-  RealType covariance = itk::NumericTraits<RealType>::Zero;
-  FrequencyType globalFrequency = histogram->GetTotalFrequency();
-  if(globalFrequency == 0)
+  if(histogram->GetTotalFrequency() == 0)
     {
     itkExceptionMacro(<<"Histogram must contain at least 1 element.");
     }
-  while (iter != end)
+    
+  for( unsigned int noDimX = 0; noDimX < NumberOfDimension; noDimX++ )
+  for( unsigned int noDimY = 0; noDimY < NumberOfDimension; noDimY++ )
     {
-    covariance +=    static_cast<RealType>(iter.GetFrequency()) 
-                 * ( static_cast<RealType>(iter.GetMeasurementVector()[0])
-	            -static_cast<RealType>(m_mean)              );
-    ++iter ;
-    }
-  covariance /= static_cast<RealType>(globalFrequency);
+    MeasurementType covariance = itk::NumericTraits<MeasurementType>::Zero;
+    for (unsigned int i = 0; i < histogram->GetSize()[noDimX]; i++)
+    for (unsigned int j = 0; j < histogram->GetSize()[noDimY]; j++)
+      {
+      MeasurementType valX  = histogram->GetMeasurement(i, noDimX);
+      MeasurementType valY  = histogram->GetMeasurement(j, noDimY);
+      FrequencyType freqX = histogram->GetFrequency(i, noDimX);
+      FrequencyType freqY = histogram->GetFrequency(j, noDimY);
+
+      valX -= static_cast<MeasurementType>(m_mean[noDimX]);
+      valY -= static_cast<MeasurementType>(m_mean[noDimY]);
+      covariance += ( (valX*freqX)*(valY*freqY) );
+      }
+    covariance /=  histogram->GetTotalFrequency();
+    m_covariance[noDimX*NumberOfDimension+noDimY] = static_cast<TOutput>(covariance);
+    }  
   
-  m_covariance = static_cast<OutputType>(covariance); 
-#endif
 }
 
 template< class TInputHistogram, class TOutput >
diff --git a/Testing/Code/Common/otbHistogramStatisticsFunction.cxx b/Testing/Code/Common/otbHistogramStatisticsFunction.cxx
index 870f2efdda995915a18472d2ce1d816b7ab8bc2d..7599154dbb96693d3dfe9f641e28f138fed64b5f 100644
--- a/Testing/Code/Common/otbHistogramStatisticsFunction.cxx
+++ b/Testing/Code/Common/otbHistogramStatisticsFunction.cxx
@@ -82,8 +82,8 @@ int otbHistogramStatisticsFunction(int argc, char* argv[])
   	std::cout << "Error in mean estimation" << std::endl;
 	return EXIT_FAILURE;
   }
-/*
-  Covariance = HistogramStatisticsFunction->GetCovariance();
+
+  Covariance = HistogramStatisticsFunction->GetCovariance()[0];
   std::cout << "Covariance 1 : " << Covariance << std::endl;
 
   if( Covariance != 0 ) 
@@ -91,7 +91,7 @@ int otbHistogramStatisticsFunction(int argc, char* argv[])
   	std::cout << "Error in covariance estimation" << std::endl;
 	return EXIT_FAILURE;
   }
-*/
+
   // create histogram just all value equal to zero except the first one
   for (HistogramType::Iterator iter = histogram->Begin(); iter != histogram->End(); ++iter)
     {
@@ -124,8 +124,8 @@ int otbHistogramStatisticsFunction(int argc, char* argv[])
   	std::cout << "Error in mean estimation" << std::endl;
 	return EXIT_FAILURE;
   }
-/*
-  Covariance = HistogramStatisticsFunction->GetCovariance();
+
+  Covariance = HistogramStatisticsFunction->GetCovariance()[0];
   std::cout << "Covariance 2 : " << Covariance << std::endl;
 
   if( Covariance != 0 ) 
@@ -133,6 +133,6 @@ int otbHistogramStatisticsFunction(int argc, char* argv[])
   	std::cout << "Error in covariance estimation" << std::endl;
 	return EXIT_FAILURE;
   }
-*/  
+  
   return EXIT_SUCCESS;
 }