void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::BeforeThreadedGenerateData()
{
    typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );
    itk::Vector<double,3> spacing = ShCoeffImage->GetSpacing();
    double minSpacing = spacing[0];
    if (spacing[1]<minSpacing)
        minSpacing = spacing[1];
    if (spacing[2]<minSpacing)
        minSpacing = spacing[2];

    mitk::Point3D origin = ShCoeffImage->GetOrigin();
    itk::Matrix<double, 3, 3> direction = ShCoeffImage->GetDirection();
    ImageRegion<3> imageRegion = ShCoeffImage->GetLargestPossibleRegion();

    if (m_MaskImage.IsNotNull())
    {
        origin = m_MaskImage->GetOrigin();
        direction = m_MaskImage->GetDirection();
        imageRegion = m_MaskImage->GetLargestPossibleRegion();
    }

    m_DirectionImageContainer = ItkDirectionImageContainer::New();
    for (unsigned int i=0; i<m_MaxNumPeaks; i++)
    {
        itk::Vector< float, 3 > nullVec; nullVec.Fill(0.0);
        ItkDirectionImage::Pointer img = ItkDirectionImage::New();
        img->SetSpacing( spacing );
        img->SetOrigin( origin );
        img->SetDirection( direction );
        img->SetRegions( imageRegion );
        img->Allocate();
        img->FillBuffer(nullVec);
        m_DirectionImageContainer->InsertElement(m_DirectionImageContainer->Size(), img);
    }
    if (m_MaskImage.IsNull())
    {
        m_MaskImage = ItkUcharImgType::New();
        m_MaskImage->SetSpacing( spacing );
        m_MaskImage->SetOrigin( origin );
        m_MaskImage->SetDirection( direction );
        m_MaskImage->SetRegions( imageRegion );
        m_MaskImage->Allocate();
        m_MaskImage->FillBuffer(1);
    }
    m_NumDirectionsImage = ItkUcharImgType::New();
    m_NumDirectionsImage->SetSpacing( spacing );
    m_NumDirectionsImage->SetOrigin( origin );
    m_NumDirectionsImage->SetDirection( direction );
    m_NumDirectionsImage->SetRegions( imageRegion );
    m_NumDirectionsImage->Allocate();
    m_NumDirectionsImage->FillBuffer(0);

    this->SetNumberOfIndexedOutputs(m_MaxNumPeaks);

    // calculate SH basis
    OdfType odf;
    vnl_matrix_fixed<double, 3, NrOdfDirections>* directions = odf.GetDirections();
    vnl_matrix< double > sphCoords;
    std::vector< DirectionType > dirs;
    for (int i=0; i<NrOdfDirections; i++)
        dirs.push_back(directions->get_column(i));
    Cart2Sph(dirs, sphCoords);                          // convert candidate peaks to spherical angles
    m_ShBasis = CalcShBasis(sphCoords);                // evaluate spherical harmonics at each peak

    MITK_INFO << "Starting finite differences maximum extraction";
    MITK_INFO << "ODF sampling points: " << NrOdfDirections;
    MITK_INFO << "SH order: " << ShOrder;
    MITK_INFO << "Maximum peaks: " << m_MaxNumPeaks;
    MITK_INFO << "Relative threshold: " << m_PeakThreshold;
    MITK_INFO << "Absolute threshold: " << m_AbsolutePeakThreshold;
    MITK_INFO << "Clustering threshold: " << m_ClusteringThreshold;
    MITK_INFO << "Angular threshold: " << m_AngularThreshold;
}
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::ThreadedGenerateData( const OutputImageRegionType& outputRegionForThread, ThreadIdType threadID )
{
    typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );

    ImageRegionConstIterator< CoefficientImageType > cit(ShCoeffImage, outputRegionForThread );

    OdfType odf;
    while( !cit.IsAtEnd() )
    {
        typename CoefficientImageType::IndexType index = cit.GetIndex();
        if (m_MaskImage->GetPixel(index)==0)
        {
            ++cit;
            continue;
        }

        CoefficientPixelType c = cit.Get();

        // calculate ODF
        double max = 0;
        odf.Fill(0.0);
        for (int i=0; i<NrOdfDirections; i++)
        {
            for (int j=0; j<m_NumCoeffs; j++)
                odf[i] += c[j]*m_ShBasis(i,j);
            if (odf[i]>max)
                max = odf[i];
        }
        if (max<0.0001)
        {
            ++cit;
            continue;
        }

        std::vector< DirectionType > candidates, peaks, temp;
        peaks.clear();
        max *= m_PeakThreshold;                         // relative threshold
        FindCandidatePeaks(odf, max, candidates);       // find all local maxima
        candidates = MeanShiftClustering(candidates);   // cluster maxima

        vnl_matrix< double > shBasis, sphCoords;
        Cart2Sph(candidates, sphCoords);                // convert candidate peaks to spherical angles
        shBasis = CalcShBasis(sphCoords);            // evaluate spherical harmonics at each peak
        max = 0.0;
        for (unsigned int i=0; i<candidates.size(); i++)         // scale peaks according to ODF value
        {
            double val = 0;
            for (int j=0; j<m_NumCoeffs; j++)
                val += c[j]*shBasis(i,j);
            if (val>max)
                max = val;
            peaks.push_back(candidates[i]*val);
        }
        std::sort( peaks.begin(), peaks.end(), CompareVectors );  // sort peaks

        // kick out directions to close to a larger direction (too far away to cluster but too close to keep)
        unsigned int m = peaks.size();
        if ( m>m_DirectionImageContainer->Size() )
            m = m_DirectionImageContainer->Size();
        for (unsigned int i=0; i<m; i++)
        {
            DirectionType v1 = peaks.at(i);
            double val = v1.magnitude();
            if (val<max*m_PeakThreshold || val<m_AbsolutePeakThreshold)
                break;

            bool flag = true;
            for (unsigned int j=0; j<peaks.size(); j++)
                if (i!=j)
                {
                    DirectionType v2 = peaks.at(j);
                    double val2 = v2.magnitude();
                    double angle = fabs(dot_product(v1,v2)/(val*val2));
                    if (angle>m_AngularThreshold && val<val2)
                    {
                        flag = false;
                        break;
                    }
                }

            if (flag)
                temp.push_back(v1);
        }
        peaks = temp;

        // fill output image
        unsigned int num = peaks.size();
        if ( num>m_DirectionImageContainer->Size() )
            num = m_DirectionImageContainer->Size();
        for (unsigned int i=0; i<num; i++)
        {
            vnl_vector<double> dir = peaks.at(i);

            ItkDirectionImage::Pointer img = m_DirectionImageContainer->GetElement(i);

            switch (m_NormalizationMethod)
            {
            case NO_NORM:
                break;
            case SINGLE_VEC_NORM:
                dir.normalize();
                break;
            case MAX_VEC_NORM:
                dir /= max;
                break;
            }

            dir = m_MaskImage->GetDirection()*dir;
            itk::Vector< float, 3 > pixel;
            pixel.SetElement(0, -dir[0]);
            pixel.SetElement(1, dir[1]);
            pixel.SetElement(2, dir[2]);
            img->SetPixel(index, pixel);
        }
        m_NumDirectionsImage->SetPixel(index, num);
        ++cit;
    }
    MITK_INFO << "Thread " << threadID << " finished extraction";
}
void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::BeforeThreadedGenerateData()
{
  typename CoefficientImageType::Pointer ShCoeffImage = static_cast< CoefficientImageType* >( this->ProcessObject::GetInput(0) );
  itk::Vector<double,3> spacing = ShCoeffImage->GetSpacing();
  double minSpacing = spacing[0];
  if (spacing[1]<minSpacing)
    minSpacing = spacing[1];
  if (spacing[2]<minSpacing)
    minSpacing = spacing[2];

  mitk::Point3D origin = ShCoeffImage->GetOrigin();
  itk::Matrix<double, 3, 3> direction = ShCoeffImage->GetDirection();
  ImageRegion<3> imageRegion = ShCoeffImage->GetLargestPossibleRegion();

  if (m_MaskImage.IsNotNull())
  {
    origin = m_MaskImage->GetOrigin();
    direction = m_MaskImage->GetDirection();
    imageRegion = m_MaskImage->GetLargestPossibleRegion();
  }

  itk::Vector<double, 3> spacing3 = ShCoeffImage->GetSpacing();
  itk::Point<float, 3> origin3 = ShCoeffImage->GetOrigin();
  itk::Matrix<double, 3, 3> direction3 = ShCoeffImage->GetDirection();
  itk::ImageRegion<3> imageRegion3 = ShCoeffImage->GetLargestPossibleRegion();

  itk::Vector<double, 4> spacing4;
  itk::Point<float, 4> origin4;
  itk::Matrix<double, 4, 4> direction4;
  itk::ImageRegion<4> imageRegion4;

  spacing4[0] = spacing3[0]; spacing4[1] = spacing3[1]; spacing4[2] = spacing3[2]; spacing4[3] = 1;
  origin4[0] = origin3[0]; origin4[1] = origin3[1]; origin4[2] = origin3[2]; origin4[3] = 0;
  for (int r=0; r<3; r++)
    for (int c=0; c<3; c++)
      direction4[r][c] = direction3[r][c];
  direction4[3][3] = 1;
  imageRegion4.SetSize(0, imageRegion3.GetSize()[0]);
  imageRegion4.SetSize(1, imageRegion3.GetSize()[1]);
  imageRegion4.SetSize(2, imageRegion3.GetSize()[2]);
  imageRegion4.SetSize(3, m_MaxNumPeaks*3);

  m_PeakImage = PeakImageType::New();
  m_PeakImage->SetSpacing( spacing4 );
  m_PeakImage->SetOrigin( origin4 );
  m_PeakImage->SetDirection( direction4 );
  m_PeakImage->SetRegions( imageRegion4 );
  m_PeakImage->Allocate();
  m_PeakImage->FillBuffer(0.0);

  if (m_MaskImage.IsNull())
  {
    m_MaskImage = ItkUcharImgType::New();
    m_MaskImage->SetSpacing( spacing );
    m_MaskImage->SetOrigin( origin );
    m_MaskImage->SetDirection( direction );
    m_MaskImage->SetRegions( imageRegion );
    m_MaskImage->Allocate();
    m_MaskImage->FillBuffer(1);
  }
  m_NumDirectionsImage = ItkUcharImgType::New();
  m_NumDirectionsImage->SetSpacing( spacing );
  m_NumDirectionsImage->SetOrigin( origin );
  m_NumDirectionsImage->SetDirection( direction );
  m_NumDirectionsImage->SetRegions( imageRegion );
  m_NumDirectionsImage->Allocate();
  m_NumDirectionsImage->FillBuffer(0);

  // calculate SH basis
  OdfType odf;
  vnl_matrix< double > sphCoords;
  std::vector< DirectionType > dirs;
  for (int i=0; i<NrOdfDirections; i++)
  {
    DirectionType odf_dir;
    odf_dir[0] = odf.GetDirection(i)[0];
    odf_dir[1] = odf.GetDirection(i)[1];
    odf_dir[2] = odf.GetDirection(i)[2];
    dirs.push_back(odf_dir);
  }
  Cart2Sph(dirs, sphCoords);                          // convert candidate peaks to spherical angles
  m_ShBasis = CalcShBasis(sphCoords);                // evaluate spherical harmonics at each peak

  MITK_INFO << "Starting finite differences maximum extraction";
  MITK_INFO << "ODF sampling points: " << NrOdfDirections;
  MITK_INFO << "SH order: " << ShOrder;
  MITK_INFO << "Maximum peaks: " << m_MaxNumPeaks;
  MITK_INFO << "Relative threshold: " << m_PeakThreshold;
  MITK_INFO << "Absolute threshold: " << m_AbsolutePeakThreshold;
  MITK_INFO << "Clustering threshold: " << m_ClusteringThreshold;
  MITK_INFO << "Angular threshold: " << m_AngularThreshold;
}