ItkImageType2D::Pointer mitk::ToFCompositeFilter::ProcessItkBilateralFilter(ItkImageType2D::Pointer inputItkImage)
{
  ItkImageType2D::Pointer outputItkImage;
  BilateralFilterType::Pointer bilateralFilter = BilateralFilterType::New();
  bilateralFilter->SetInput(inputItkImage);
  bilateralFilter->SetDomainSigma(m_BilateralFilterDomainSigma);
  bilateralFilter->SetRangeSigma(m_BilateralFilterRangeSigma);
  //bilateralFilter->SetRadius(m_BilateralFilterKernelRadius);
  outputItkImage = bilateralFilter->GetOutput();
  outputItkImage->Update();
  return outputItkImage;
}
void mitk::ToFCompositeFilter::GenerateData()
{
  // copy input 1...n to output 1...n
  for (unsigned int idx=0; idx<this->GetNumberOfOutputs(); idx++)
  {
    mitk::Image::Pointer outputImage = this->GetOutput(idx);
    mitk::Image::Pointer inputImage = this->GetInput(idx);
    if (outputImage.IsNotNull()&&inputImage.IsNotNull())
    {
      ImageReadAccessor inputAcc(inputImage, inputImage->GetSliceData());
      outputImage->CopyInformation(inputImage);
      outputImage->Initialize(inputImage->GetPixelType(),inputImage->GetDimension(),inputImage->GetDimensions());
      outputImage->SetSlice(inputAcc.GetData());
    }
  }
  //mitk::Image::Pointer outputDistanceImage = this->GetOutput();
  ImageReadAccessor outputAcc(this->GetOutput(), this->GetOutput()->GetSliceData(0, 0, 0) );
  float* outputDistanceFloatData = (float*) outputAcc.GetData();

  //mitk::Image::Pointer inputDistanceImage = this->GetInput();
  ImageReadAccessor inputAcc(this->GetInput(), this->GetInput()->GetSliceData(0, 0, 0) );

  // copy initial distance image to ipl image
  float* distanceFloatData = (float*)inputAcc.GetData();
  memcpy(this->m_IplDistanceImage->imageData, (void*)distanceFloatData, this->m_ImageSize);
  if (m_ApplyThresholdFilter||m_ApplyMaskSegmentation)
  {
    ProcessSegmentation(this->m_IplDistanceImage);
  }
  if (this->m_ApplyTemporalMedianFilter||this->m_ApplyAverageFilter)
  {
    ProcessStreamedQuickSelectMedianImageFilter(this->m_IplDistanceImage);
  }
  if (this->m_ApplyMedianFilter)
  {
    ProcessCVMedianFilter(this->m_IplDistanceImage, this->m_IplOutputImage);
    memcpy( this->m_IplDistanceImage->imageData, this->m_IplOutputImage->imageData, this->m_ImageSize );
  }
  if (this->m_ApplyBilateralFilter)
  {
      float* itkFloatData = this->m_ItkInputImage->GetBufferPointer();
      memcpy(itkFloatData, this->m_IplDistanceImage->imageData, this->m_ImageSize );
      ItkImageType2D::Pointer itkOutputImage = ProcessItkBilateralFilter(this->m_ItkInputImage);
      memcpy( this->m_IplDistanceImage->imageData, itkOutputImage->GetBufferPointer(), this->m_ImageSize );

    //ProcessCVBilateralFilter(this->m_IplDistanceImage, this->m_OutputIplImage, domainSigma, rangeSigma, kernelRadius);
    //memcpy( distanceFloatData, this->m_OutputIplImage->imageData, distanceImageSize );
  }
  memcpy( outputDistanceFloatData, this->m_IplDistanceImage->imageData, this->m_ImageSize );
}
void mitk::ToFCompositeFilter::CreateItkImage(ItkImageType2D::Pointer &itkInputImage)
{
  itkInputImage = ItkImageType2D::New();
  ItkImageType2D::IndexType startIndex;
  startIndex[0] =   0;  // first index on X
  startIndex[1] =   0;  // first index on Y
  ItkImageType2D::SizeType  size;
  size[0]  = this->m_ImageWidth;  // size along X
  size[1]  = this->m_ImageHeight;  // size along Y
  ItkImageType2D::RegionType region;
  region.SetSize( size );
  region.SetIndex( startIndex );
  itkInputImage->SetRegions( region );
  itkInputImage->Allocate();

}
inline static mitk::Image::Pointer CreateTestImageWithPointSet(mitk::ScalarType pixelValue, unsigned int dimX, unsigned int dimY, mitk::PointSet::Pointer subSet)
{
  typedef itk::Image<mitk::ScalarType,2> ItkImageType2D;
  typedef itk::ImageRegionIterator<ItkImageType2D> ItkImageRegionIteratorType2D;

  ItkImageType2D::Pointer image = ItkImageType2D::New();
  ItkImageType2D::IndexType start;
  start[0] = 0;
  start[1] = 0;
  ItkImageType2D::SizeType size;
  size[0] = dimX;
  size[1] = dimY;
  ItkImageType2D::RegionType region;
  region.SetSize(size);
  region.SetIndex( start);
  ItkImageType2D::SpacingType spacing;
  spacing[0] = 1.0;
  spacing[1] = 1.0;

  image->SetRegions( region );
  image->SetSpacing ( spacing );
  image->Allocate();

  //Obtaining image data from ToF camera//

  //Correlate inten values to PixelIndex//
  ItkImageRegionIteratorType2D imageIterator(image,image->GetLargestPossibleRegion());
  imageIterator.GoToBegin();

  while (!imageIterator.IsAtEnd())
  {
    imageIterator.Set(pixelValue);
    ++imageIterator;
  }
  // distances varying from pixelValue
  std::vector<mitk::ScalarType> distances;
  distances.push_back(50);
  distances.push_back(500);
  distances.push_back(2050);
  distances.push_back(300);
  // set the pixel values for the subset
  for (unsigned int i=0; i<subSet->GetSize(); i++)
  {
    mitk::Point3D point = subSet->GetPoint(i);
    ItkImageType2D::IndexType index;
    index[0] = point[0];
    index[1] = point[1];
    mitk::ScalarType distance = distances.at(i);
    image->SetPixel(index,distance);
  }
  mitk::Image::Pointer mitkImage = mitk::Image::New();
  mitk::CastToMitkImage(image,mitkImage);
  return mitkImage;
}