void LidarSegmentationWidget::on_action_View_DepthImage_triggered()
{
  //Helpers::ITKImageChannelToVTKImage(thresholdFilter->GetOutput(), 3, this->OriginalImageData);
  
  typedef itk::Image<ImageType::InternalPixelType, 2> ScalarImageType;
  ScalarImageType::Pointer depthImage = ScalarImageType::New();
  
  ITKHelpers::ExtractChannel(this->Image.GetPointer(), 3, depthImage.GetPointer());
  
  typedef itk::ThresholdImageFilter<ScalarImageType> ThresholdImageFilterType;

  float maxDepth = 20.0f;
  ThresholdImageFilterType::Pointer thresholdFilter = ThresholdImageFilterType::New();
  thresholdFilter->SetInput(depthImage);
  thresholdFilter->ThresholdAbove(maxDepth);
  thresholdFilter->SetOutsideValue(maxDepth);
  thresholdFilter->Update();
  
  ITKVTKHelpers::ITKScalarImageToScaledVTKImage(thresholdFilter->GetOutput(), this->OriginalImageData);
  
  Refresh();
}
int main(int argc, char *argv[])
{
  unsigned int t = time(NULL);
  srand(t);
  
  itk::Size<2> size;
  size.Fill(10);

  itk::Index<2> index;
  index.Fill(0);

  itk::ImageRegion<2> region(index, size);
/*
  // Generate a random image (this method doesn't work with VectorImage)
  itk::RandomImageSource<FloatVectorImageType>::Pointer imageSource =
    itk::RandomImageSource<FloatVectorImageType>::New();
  imageSource->SetNumberOfThreads(1); // to produce non-random results
  imageSource->SetSize(size);
  imageSource->SetMin(0);
  imageSource->SetMax(100);
  imageSource->Update();
  FloatVectorImageType::Pointer image = imageSource->GetOutput();
*/
  // Generate a random image
  FloatVectorImageType::Pointer image = FloatVectorImageType::New();
  image->SetRegions(region);
  image->SetNumberOfComponentsPerPixel(3);
  image->Allocate();

  {
  itk::ImageRegionIterator<FloatVectorImageType> imageIterator(image, image->GetLargestPossibleRegion());

  while(!imageIterator.IsAtEnd())
    {
    FloatVectorImageType::PixelType pixel;
    pixel.SetSize(3);
    pixel[0] = RandomFloat();
    pixel[1] = RandomFloat();
    pixel[2] = RandomFloat();
    imageIterator.Set(pixel);
    ++imageIterator;
    }
  }
  
  // Write the image
  itk::ImageFileWriter<FloatVectorImageType>::Pointer imageWriter =
    itk::ImageFileWriter<FloatVectorImageType>::New();
  imageWriter->SetFileName("image.mha");
  imageWriter->SetInput(image);
  imageWriter->Update();

  // Generate a random mask
  itk::RandomImageSource<Mask>::Pointer maskSource =
    itk::RandomImageSource<Mask>::New();
  maskSource->SetNumberOfThreads(1); // to produce non-random results
  maskSource->SetSize(size);
  maskSource->SetMin(0);
  maskSource->SetMax(255);
  maskSource->Update();

  // Threshold the mask
  //typedef itk::ThresholdImageFilter <UnsignedCharImageType> ThresholdImageFilterType;
  typedef itk::BinaryThresholdImageFilter <Mask, Mask> ThresholdImageFilterType;
  ThresholdImageFilterType::Pointer thresholdFilter = ThresholdImageFilterType::New();
  thresholdFilter->SetInput(maskSource->GetOutput());
  thresholdFilter->SetLowerThreshold(0);
  thresholdFilter->SetUpperThreshold(122);
  thresholdFilter->SetOutsideValue(1);
  thresholdFilter->SetInsideValue(0);
  thresholdFilter->Update();
  Mask::Pointer mask = thresholdFilter->GetOutput();

  // Write the mask
  itk::ImageFileWriter<Mask>::Pointer maskWriter =
    itk::ImageFileWriter<Mask>::New();
  maskWriter->SetFileName("mask.png");
  maskWriter->SetInput(mask);
  maskWriter->Update();

  unsigned int patchRadius = 1;
  // Create source patches
  itk::ImageRegionConstIterator<FloatVectorImageType> imageIterator(image, image->GetLargestPossibleRegion());
  std::vector<itk::ImageRegion<2> > sourcePatches;
  while(!imageIterator.IsAtEnd())
    {
    itk::Index<2> currentPixel = imageIterator.GetIndex();
    itk::ImageRegion<2> region = GetRegionInRadiusAroundPixel(currentPixel, patchRadius);
    if(image->GetLargestPossibleRegion().IsInside(region))
      {
      sourcePatches.push_back(region);
      }
    ++imageIterator;
    }

  itk::Size<2> targetSize;
  targetSize.Fill(patchRadius * 2 + 1);

  itk::Index<2> targetIndex;
  targetIndex.Fill(3);
  
  itk::ImageRegion<2> targetRegion(targetIndex, targetSize);
  SelfPatchCompare patchCompare;
  patchCompare.SetImage(image);
  patchCompare.SetMask(mask);
  patchCompare.SetSourceRegions(sourcePatches);
  patchCompare.SetTargetRegion(targetRegion);
  patchCompare.ComputeOffsets();
  
  //unsigned int bestMatchSourcePatchId = patchCompare.FindBestPatch();
  //std::cout << "bestMatchSourcePatchId: " << bestMatchSourcePatchId << std::endl;

  unsigned int patchId = 1;
  float slowPatchDifference = patchCompare.SlowDifference(sourcePatches[patchId]);
  std::cout << "slowPatchDifference: " << slowPatchDifference << std::endl;
  
  float fastPatchDifference = patchCompare.PatchDifference(sourcePatches[patchId]);
  std::cout << "fastPatchDifference: " << fastPatchDifference << std::endl;

  unsigned int iterations = 1e6;

  itk::TimeProbe slowTimer;
  slowTimer.Start();
  
  for(unsigned int i = 0; i < iterations; ++i)
    {
    float slowPatchDifference = patchCompare.SlowDifference(sourcePatches[patchId]);
    }

  slowTimer.Stop();
  std::cout << "Slow Total: " << slowTimer.GetTotal() << std::endl;

  itk::TimeProbe fastTimer;
  fastTimer.Start();

  for(unsigned int i = 0; i < iterations; ++i)
    {
    float fastPatchDifference = patchCompare.PatchDifference(sourcePatches[patchId]);
    }

  fastTimer.Stop();
  std::cout << "Fast Total: " << fastTimer.GetTotal() << std::endl;
  return EXIT_SUCCESS;
}