bool mitk::NavigationDataLandmarkTransformFilter::FindCorrespondentLandmarks(LandmarkPointContainer& sources, const LandmarkPointContainer& targets) const
{
  if (sources.size() < 6 || targets.size() < 6)
    return false;
  //throw std::invalid_argument("ICP correspondence finding needs at least 6 landmarks");

  /* lots of type definitions */
  typedef itk::PointSet<mitk::ScalarType, 3> PointSetType;
  //typedef itk::BoundingBox<PointSetType::PointIdentifier, PointSetType::PointDimension> BoundingBoxType;

  typedef itk::EuclideanDistancePointMetric< PointSetType, PointSetType> MetricType;
  //typedef MetricType::TransformType TransformBaseType;
  //typedef MetricType::TransformType::ParametersType ParametersType;
  //typedef TransformBaseType::JacobianType JacobianType;
  //typedef itk::Euler3DTransform< double > TransformType;
  typedef itk::VersorRigid3DTransform< double > TransformType;
  typedef TransformType ParametersType;
  typedef itk::PointSetToPointSetRegistrationMethod< PointSetType, PointSetType > RegistrationType;

  /* copy landmarks to itk pointsets for registration */
  PointSetType::Pointer sourcePointSet = PointSetType::New();
  unsigned int i = 0;
  for (LandmarkPointContainer::const_iterator it = sources.begin(); it != sources.end(); ++it)
  {
    PointSetType::PointType doublePoint;
    mitk::itk2vtk(*it, doublePoint); // copy mitk::ScalarType point into double point as workaround to ITK 3.10 bug
    sourcePointSet->SetPoint(i++, doublePoint /**it*/);
  }

  i = 0;
  PointSetType::Pointer targetPointSet = PointSetType::New();
  for (LandmarkPointContainer::const_iterator it = targets.begin(); it != targets.end(); ++it)
  {
    PointSetType::PointType doublePoint;
    mitk::itk2vtk(*it, doublePoint); // copy mitk::ScalarType point into double point as workaround to ITK 3.10 bug
    targetPointSet->SetPoint(i++, doublePoint /**it*/);
  }

  /* get centroid and extends of our pointsets */
  //BoundingBoxType::Pointer sourceBoundingBox = BoundingBoxType::New();
  //sourceBoundingBox->SetPoints(sourcePointSet->GetPoints());
  //sourceBoundingBox->ComputeBoundingBox();
  //BoundingBoxType::Pointer targetBoundingBox = BoundingBoxType::New();
  //targetBoundingBox->SetPoints(targetPointSet->GetPoints());
  //targetBoundingBox->ComputeBoundingBox();


  TransformType::Pointer transform = TransformType::New();
  transform->SetIdentity();
  //transform->SetTranslation(targetBoundingBox->GetCenter() - sourceBoundingBox->GetCenter());

  itk::LevenbergMarquardtOptimizer::Pointer optimizer = itk::LevenbergMarquardtOptimizer::New();
  optimizer->SetUseCostFunctionGradient(false);

  RegistrationType::Pointer registration = RegistrationType::New();

  // Scale the translation components of the Transform in the Optimizer
  itk::LevenbergMarquardtOptimizer::ScalesType scales(transform->GetNumberOfParameters());
  const double translationScale = 5000; //sqrtf(targetBoundingBox->GetDiagonalLength2())  * 1000; // dynamic range of translations
  const double rotationScale = 1.0; // dynamic range of rotations
  scales[0] = 1.0 / rotationScale;
  scales[1] = 1.0 / rotationScale;
  scales[2] = 1.0 / rotationScale;
  scales[3] = 1.0 / translationScale;
  scales[4] = 1.0 / translationScale;
  scales[5] = 1.0 / translationScale;
  //scales.Fill(0.01);
  unsigned long numberOfIterations = 80000;
  double gradientTolerance = 1e-10; // convergence criterion
  double valueTolerance = 1e-10; // convergence criterion
  double epsilonFunction = 1e-10; // convergence criterion
  optimizer->SetScales( scales );
  optimizer->SetNumberOfIterations( numberOfIterations );
  optimizer->SetValueTolerance( valueTolerance );
  optimizer->SetGradientTolerance( gradientTolerance );
  optimizer->SetEpsilonFunction( epsilonFunction );


  registration->SetInitialTransformParameters( transform->GetParameters() );
  //------------------------------------------------------
  // Connect all the components required for Registration
  //------------------------------------------------------
  MetricType::Pointer metric = MetricType::New();

  registration->SetMetric( metric );
  registration->SetOptimizer( optimizer );
  registration->SetTransform( transform );
  registration->SetFixedPointSet( targetPointSet );
  registration->SetMovingPointSet( sourcePointSet );

  try
  {
    //registration->StartRegistration();
    registration->Update();
  }
  catch( itk::ExceptionObject & e )
  {
    MITK_INFO << "Exception caught during ICP optimization: " << e;
    return false;
    //throw e;
  }
  MITK_INFO << "ICP successful: Solution = " << transform->GetParameters() << std::endl;
  MITK_INFO << "Metric value: " << metric->GetValue(transform->GetParameters());

  /* find point correspondences */
  //mitk::PointLocator::Pointer pointLocator = mitk::PointLocator::New();  // <<- use mitk::PointLocator instead of searching manually?
  //pointLocator->SetPoints()
  for (LandmarkPointContainer::const_iterator sourcesIt = sources.begin(); sourcesIt != sources.end(); ++sourcesIt)
  {
  }
  //MetricType::MeasureType closestDistances = metric->GetValue(transform->GetParameters());
  //unsigned int index = 0;
  LandmarkPointContainer sortedSources;
  for (LandmarkPointContainer::const_iterator targetsIt = targets.begin(); targetsIt != targets.end(); ++targetsIt)
  {
    double minDistance = itk::NumericTraits<double>::max();
    LandmarkPointContainer::iterator minDistanceIterator = sources.end();
    for (LandmarkPointContainer::iterator sourcesIt = sources.begin(); sourcesIt != sources.end(); ++sourcesIt)
    {
      TransformInitializerType::LandmarkPointType transformedSource = transform->TransformPoint(*sourcesIt);
      double dist = targetsIt->EuclideanDistanceTo(transformedSource);
      MITK_INFO << "target: " << *targetsIt << ", source: " << *sourcesIt << ", transformed source: " << transformedSource << ", dist: " << dist;
      if (dist < minDistance )
      {
        minDistanceIterator = sourcesIt;
        minDistance = dist;
      }
    }
    if (minDistanceIterator == sources.end())
      return false;
    MITK_INFO << "minimum distance point is: " << *minDistanceIterator << " (dist: " << targetsIt->EuclideanDistanceTo(transform->TransformPoint(*minDistanceIterator)) << ", minDist: " << minDistance << ")";
    sortedSources.push_back(*minDistanceIterator); // this point is assigned
    sources.erase(minDistanceIterator); // erase it from sources to avoid duplicate assigns
  }
  //for (LandmarkPointContainer::const_iterator sortedSourcesIt = sortedSources.begin(); targetsIt != sortedSources.end(); ++targetsIt)
  sources = sortedSources;
  return true;
}
void MutualInformationRegistration::updateRegistration() {
    if(!isReady())
        return;

    convertVolumes();

    //typedef itk::RegularStepGradientDescentOptimizer OptimizerType;
    typedef itk::VersorRigid3DTransformOptimizer OptimizerType;
    typedef OptimizerType::ScalesType       OptimizerScalesType;
    typedef itk::LinearInterpolateImageFunction< InternalImageType, double             > InterpolatorType;
    typedef itk::MattesMutualInformationImageToImageMetric< InternalImageType, InternalImageType >   MetricType;
    typedef itk::MultiResolutionImageRegistrationMethod< InternalImageType, InternalImageType >   RegistrationType;

    typedef itk::MultiResolutionPyramidImageFilter< InternalImageType, InternalImageType >   FixedImagePyramidType;
    typedef itk::MultiResolutionPyramidImageFilter< InternalImageType, InternalImageType >   MovingImagePyramidType;

    OptimizerType::Pointer      optimizer     = OptimizerType::New();
    InterpolatorType::Pointer   interpolator  = InterpolatorType::New();
    RegistrationType::Pointer   registration  = RegistrationType::New();
    MetricType::Pointer         metric        = MetricType::New();

    FixedImagePyramidType::Pointer fixedImagePyramid = FixedImagePyramidType::New();
    MovingImagePyramidType::Pointer movingImagePyramid = MovingImagePyramidType::New();

    registration->SetOptimizer(optimizer);
    registration->SetTransform(transform_);
    registration->SetInterpolator(interpolator);
    registration->SetMetric(metric);
    registration->SetFixedImagePyramid(fixedImagePyramid);
    registration->SetMovingImagePyramid(movingImagePyramid);

    OptimizerScalesType optimizerScales( transform_->GetNumberOfParameters() );

    float rotScale = 1.0 / 1000.0f;
    optimizerScales[0] = 1.0f;
    optimizerScales[1] = 1.0f;
    optimizerScales[2] = 1.0f;
    optimizerScales[3] = rotScale;
    optimizerScales[4] = rotScale;
    optimizerScales[5] = rotScale;
    optimizer->SetScales( optimizerScales );
    optimizer->SetMaximumStepLength(0.2);
    optimizer->SetMinimumStepLength(0.0001);

    InternalImageType::Pointer fixed = voreenToITK<float>(fixedVolumeFloat_);
    InternalImageType::Pointer moving = voreenToITK<float>(movingVolumeFloat_);
    registration->SetFixedImage(fixed);
    registration->SetMovingImage(moving);
    registration->SetFixedImageRegion( fixed->GetBufferedRegion() );

    registration->SetInitialTransformParameters( transform_->GetParameters() );

    metric->SetNumberOfHistogramBins(numHistogramBins_.get());

    size_t numVoxels = hmul(fixedVolumeFloat_->getDimensions());
    metric->SetNumberOfSpatialSamples(numVoxels * numSamples_.get());

    metric->ReinitializeSeed( 76926294 );

    //// Define whether to calculate the metric derivative by explicitly
    //// computing the derivatives of the joint PDF with respect to the Transform
    //// parameters, or doing it by progressively accumulating contributions from
    //// each bin in the joint PDF.
    metric->SetUseExplicitPDFDerivatives(explicitPDF_.get());

    optimizer->SetNumberOfIterations(numIterations_.get());

    optimizer->SetRelaxationFactor(relaxationFactor_.get());

    // Create the Command observer and register it with the optimizer.
    CommandIterationUpdate::Pointer observer = CommandIterationUpdate::New();
    optimizer->AddObserver( itk::IterationEvent(), observer );

    typedef RegistrationInterfaceCommand<RegistrationType> CommandType;
    CommandType::Pointer command = CommandType::New();
    registration->AddObserver( itk::IterationEvent(), command );

    registration->SetNumberOfLevels(numLevels_.get());

    try
    {
        registration->StartRegistration();
        std::cout << "Optimizer stop condition: " << registration->GetOptimizer()->GetStopConditionDescription() << std::endl;
    }
    catch( itk::ExceptionObject & err )
    {
        std::cout << "ExceptionObject caught !" << std::endl;
        std::cout << err << std::endl;
        //return EXIT_FAILURE;
    }

    ParametersType finalParameters = registration->GetLastTransformParameters();
    transform_->SetParameters(finalParameters);

    unsigned int numberOfIterations = optimizer->GetCurrentIteration();

    double bestValue = optimizer->GetValue();

    // Print out results
    std::cout << "Result = " << std::endl;
    std::cout << " Versor " << finalParameters[0] << " " << finalParameters[1] << " " << finalParameters[2] << std::endl;
    std::cout << " Translation " << finalParameters[1] << " " << finalParameters[4] << " " << finalParameters[5] << std::endl;
    std::cout << " Iterations    = " << numberOfIterations << std::endl;
    std::cout << " Metric value  = " << bestValue          << std::endl;

    invalidate(INVALID_RESULT);
}
void
SoItkMeanSquaresImageToImageMetric::evaluate()
{
    if( mOutput )
    {
        mOutput->unref();
        mOutput = 0;
        SO_ENGINE_OUTPUT( Output, SoItkSFDataImageMetric, setValue( 0 ) );
    }
    
	/*
    if( mGradientImage )
    {
        mGradientImage->unref();
        mGradientImage = 0;
        SO_ENGINE_OUTPUT( GradientImage, SoItkSFDataImage, setValue( 0 ) );
    }
    */

    try
    {
        switch( PixelType.getValue() )
		{
			case FLOAT:
			{
				switch( Dimension.getValue() )
				{
					case 2:
					{
                        typedef itk::Image< float, 2 > InputImageType;
						typedef itk::Image< float, 2 > OutputImageType;
                        typedef itk::MeanSquaresImageToImageMetric< InputImageType, OutputImageType > MetricType;
                        typedef itk::Transform< double, 2, 2 > TransformType;
						typedef itk::InterpolateImageFunction< InputImageType, double > InterpolatorType;
                        
                        MetricType::Pointer metric = MetricType::New();
                        metric->Register();
                        
                        // Fixed Image

						/*
                        SoItkDataImage* FixedImage_ = FixedImage.getValue();
						InputImageType* itkFixedImage =
							static_cast< InputImageType* >( FixedImage_->getPointer() );
						if( !itkFixedImage )
							return ;
						metric->SetFixedImage( itkFixedImage );
                        
                        // Moving Image
                        SoItkDataImage* MovingImage_ = MovingImage.getValue();
						InputImageType* itkMovingImage =
							static_cast< InputImageType* >( MovingImage_->getPointer() );
						if( !itkMovingImage )
							return ;
						metric->SetMovingImage( itkMovingImage );
                        
                        // Transform
                        SoItkDataTransform* Transform_ = Transform.getValue();
						TransformType* itkTransform =
							static_cast< TransformType* >( Transform_->getPointer() );
						if( !itkTransform )
							return ;
						metric->SetTransform( itkTransform );
                        
                        // Interpolator
                        SoItkDataInterpolator* Interpolator_ = Interpolator.getValue();
						InterpolatorType* itkInterpolator =
							static_cast< InterpolatorType* >( Interpolator_->getPointer() );
						if( !itkInterpolator )
							return ;
						metric->SetInterpolator( itkInterpolator );
                        
                        // Fixed Image Region
                        typedef itk::ImageRegion< 2 > ImageRegionType;
                        typedef itk::Index< 2 > ImageRegionIndexType;
                        typedef itk::Size< 2 > ImageRegionSizeType;
                        
                        ImageRegionIndexType fixedImageRegionIndex;
                        fixedImageRegionIndex[0] = FixedImageRegionOrigin.getValue()[0];
                        fixedImageRegionIndex[1] = FixedImageRegionOrigin.getValue()[1];
                        
                        ImageRegionSizeType fixedImageRegionSize;
                        fixedImageRegionSize[0] = FixedImageRegionSize.getValue()[0];
                        fixedImageRegionSize[1] = FixedImageRegionSize.getValue()[1];
                        
                        ImageRegionType fixedImageRegion;
                        fixedImageRegion.SetIndex( fixedImageRegionIndex );
                        fixedImageRegion.SetSize( fixedImageRegionSize );
                        
                        metric->SetFixedImageRegion( fixedImageRegion );
                        
                        // Transform Parameters
                        typedef itk::Array< double > ArrayType;
                        ArrayType transformParameters( (double *) TransformParameters.getValues(0), TransformParameters.getNum(), false );
                        metric->SetTransformParameters( transformParameters );
                        
                        metric->SetComputeGradient( ComputeGradient.getValue() );
        
                        if( ComputeGradient.getValue() == TRUE )
                        {
                            metric->GetGradientImage()->Register();
                            
                            mGradientImage = new SoItkDataImage( SoItkDataImage::FLOAT, 2, 
                                FixedImage.getValue()->getModelMatrix() );
                            mGradientImage->ref();
                            mGradientImage->setPointer( metric->GetGradientImage() );
                        }
						*/
                        
						mOutput = new SoItkDataImageMetric( SoItkDataImageMetric::FLOAT, 2 );
                        mOutput->ref();
                        mOutput->setPointer( metric );
					}
					break ;
                    
					case 3:
					{
						typedef itk::Image< float, 3 > InputImageType;
						typedef itk::Image< float, 3 > OutputImageType;
                        typedef itk::MeanSquaresImageToImageMetric< InputImageType, OutputImageType > MetricType;
                        typedef itk::Transform< double, 3, 3 > TransformType;
						typedef itk::InterpolateImageFunction< InputImageType, double > InterpolatorType;
                        
                        MetricType::Pointer metric = MetricType::New();
                        metric->Register();
                        
						/*

                        // Fixed Image
                        SoItkDataImage* FixedImage_ = FixedImage.getValue();
						InputImageType* itkFixedImage =
							static_cast< InputImageType* >( FixedImage_->getPointer() );
						if( !itkFixedImage )
							return ;
						metric->SetFixedImage( itkFixedImage );
                        
                        // Moving Image
                        SoItkDataImage* MovingImage_ = MovingImage.getValue();
						InputImageType* itkMovingImage =
							static_cast< InputImageType* >( MovingImage_->getPointer() );
						if( !itkMovingImage )
							return ;
						metric->SetMovingImage( itkMovingImage );
                        
                        // Transform
                        SoItkDataTransform* Transform_ = Transform.getValue();
						TransformType* itkTransform =
							static_cast< TransformType* >( Transform_->getPointer() );
						if( !itkTransform )
							return ;
						metric->SetTransform( itkTransform );
                        
                        // Interpolator
                        SoItkDataInterpolator* Interpolator_ = Interpolator.getValue();
						InterpolatorType* itkInterpolator =
							static_cast< InterpolatorType* >( Interpolator_->getPointer() );
						if( !itkInterpolator )
							return ;
						metric->SetInterpolator( itkInterpolator );
                        
                        // Fixed Image Region
                        typedef itk::ImageRegion< 3 > ImageRegionType;
                        typedef itk::Index< 3 > ImageRegionIndexType;
                        typedef itk::Size< 3 > ImageRegionSizeType;
                        
                        ImageRegionIndexType fixedImageRegionIndex;
                        fixedImageRegionIndex[0] = FixedImageRegionOrigin.getValue()[0];
                        fixedImageRegionIndex[1] = FixedImageRegionOrigin.getValue()[1];
                        fixedImageRegionIndex[2] = FixedImageRegionOrigin.getValue()[2];
                        
                        ImageRegionSizeType fixedImageRegionSize;
                        fixedImageRegionSize[0] = FixedImageRegionSize.getValue()[0];
                        fixedImageRegionSize[1] = FixedImageRegionSize.getValue()[1];
                        fixedImageRegionSize[2] = FixedImageRegionSize.getValue()[2];
                        
                        ImageRegionType fixedImageRegion;
                        fixedImageRegion.SetIndex( fixedImageRegionIndex );
                        fixedImageRegion.SetSize( fixedImageRegionSize );
                        
                        metric->SetFixedImageRegion( fixedImageRegion );
                        
                        // Transform Parameters
                        typedef itk::Array< double > ArrayType;
                        ArrayType transformParameters( (double *) TransformParameters.getValues(0), TransformParameters.getNum(), false );
                        metric->SetTransformParameters( transformParameters );
                        
                        metric->SetComputeGradient( ComputeGradient.getValue() );
        
                        if( ComputeGradient.getValue() == TRUE )
                        {
                            metric->GetGradientImage()->Register();
                            
                            mGradientImage = new SoItkDataImage( SoItkDataImage::FLOAT, 3, 
                                FixedImage.getValue()->getModelMatrix() );
                            mGradientImage->ref();
                            mGradientImage->setPointer( metric->GetGradientImage() );
                        }
						*/
                        
						mOutput = new SoItkDataImageMetric( SoItkDataImageMetric::FLOAT, 3 );
                        mOutput->ref();
                        mOutput->setPointer( metric );
					}
					break ;
				}
			}
			break ;

			case UNSIGNED_SHORT:
			{
				switch( Dimension.getValue() )
				{
					case 2:
					{
                        typedef itk::Image< unsigned short, 2 > InputImageType;
						typedef itk::Image< unsigned short, 2 > OutputImageType;
                        typedef itk::MeanSquaresImageToImageMetric< InputImageType, OutputImageType > MetricType;
                        typedef itk::Transform< double, 2, 2 > TransformType;
						typedef itk::InterpolateImageFunction< InputImageType, double > InterpolatorType;
                        
                        MetricType::Pointer metric = MetricType::New();
                        metric->Register();

                        /*

                        // Fixed Image
                        SoItkDataImage* FixedImage_ = FixedImage.getValue();
						InputImageType* itkFixedImage =
							static_cast< InputImageType* >( FixedImage_->getPointer() );
						if( !itkFixedImage )
							return ;
						metric->SetFixedImage( itkFixedImage );
                        
                        // Moving Image
                        SoItkDataImage* MovingImage_ = MovingImage.getValue();
						InputImageType* itkMovingImage =
							static_cast< InputImageType* >( MovingImage_->getPointer() );
						if( !itkMovingImage )
							return ;
						metric->SetMovingImage( itkMovingImage );
                        
                        // Transform
                        SoItkDataTransform* Transform_ = Transform.getValue();
						TransformType* itkTransform =
							static_cast< TransformType* >( Transform_->getPointer() );
						if( !itkTransform )
							return ;
						metric->SetTransform( itkTransform );
                        
                        // Interpolator
                        SoItkDataInterpolator* Interpolator_ = Interpolator.getValue();
						InterpolatorType* itkInterpolator =
							static_cast< InterpolatorType* >( Interpolator_->getPointer() );
						if( !itkInterpolator )
							return ;
						metric->SetInterpolator( itkInterpolator );
                        
                        // Fixed Image Region
                        typedef itk::ImageRegion< 2 > ImageRegionType;
                        typedef itk::Index< 2 > ImageRegionIndexType;
                        typedef itk::Size< 2 > ImageRegionSizeType;
                        
                        ImageRegionIndexType fixedImageRegionIndex;
                        fixedImageRegionIndex[0] = FixedImageRegionOrigin.getValue()[0];
                        fixedImageRegionIndex[1] = FixedImageRegionOrigin.getValue()[1];
                        
                        ImageRegionSizeType fixedImageRegionSize;
                        fixedImageRegionSize[0] = FixedImageRegionSize.getValue()[0];
                        fixedImageRegionSize[1] = FixedImageRegionSize.getValue()[1];
                        
                        ImageRegionType fixedImageRegion;
                        fixedImageRegion.SetIndex( fixedImageRegionIndex );
                        fixedImageRegion.SetSize( fixedImageRegionSize );
                        
                        metric->SetFixedImageRegion( fixedImageRegion );
                        
                        // Transform Parameters
                        typedef itk::Array< double > ArrayType;
                        ArrayType transformParameters( (double *) TransformParameters.getValues(0), TransformParameters.getNum(), false );
                        metric->SetTransformParameters( transformParameters );
                        
                        metric->SetComputeGradient( ComputeGradient.getValue() );
        
                        if( ComputeGradient.getValue() == TRUE )
                        {
                            metric->GetGradientImage()->Register();
                            
                            mGradientImage = new SoItkDataImage( SoItkDataImage::FLOAT, 2, 
                                FixedImage.getValue()->getModelMatrix() );
                            mGradientImage->ref();
                            mGradientImage->setPointer( metric->GetGradientImage() );
                        }
                        */

						mOutput = new SoItkDataImageMetric( SoItkDataImageMetric::UNSIGNED_SHORT, 2 );
                        mOutput->ref();
                        mOutput->setPointer( metric );
					}
					break ;
                    
					case 3:
					{
						typedef itk::Image< unsigned short, 3 > InputImageType;
						typedef itk::Image< unsigned short, 3 > OutputImageType;
                        typedef itk::MeanSquaresImageToImageMetric< InputImageType, OutputImageType > MetricType;
                        typedef itk::Transform< double, 3, 3 > TransformType;
						typedef itk::InterpolateImageFunction< InputImageType, double > InterpolatorType;
                        
                        MetricType::Pointer metric = MetricType::New();
                        metric->Register();
                        
						/*

                        // Fixed Image
                        SoItkDataImage* FixedImage_ = FixedImage.getValue();
						InputImageType* itkFixedImage =
							static_cast< InputImageType* >( FixedImage_->getPointer() );
						if( !itkFixedImage )
							return ;
						metric->SetFixedImage( itkFixedImage );
                        
                        // Moving Image
                        SoItkDataImage* MovingImage_ = MovingImage.getValue();
						InputImageType* itkMovingImage =
							static_cast< InputImageType* >( MovingImage_->getPointer() );
						if( !itkMovingImage )
							return ;
						metric->SetMovingImage( itkMovingImage );
                        
                        // Transform
                        SoItkDataTransform* Transform_ = Transform.getValue();
						TransformType* itkTransform =
							static_cast< TransformType* >( Transform_->getPointer() );
						if( !itkTransform )
							return ;
						metric->SetTransform( itkTransform );
                        
                        // Interpolator
                        SoItkDataInterpolator* Interpolator_ = Interpolator.getValue();
						InterpolatorType* itkInterpolator =
							static_cast< InterpolatorType* >( Interpolator_->getPointer() );
						if( !itkInterpolator )
							return ;
						metric->SetInterpolator( itkInterpolator );
                        
                        // Fixed Image Region
                        typedef itk::ImageRegion< 3 > ImageRegionType;
                        typedef itk::Index< 3 > ImageRegionIndexType;
                        typedef itk::Size< 3 > ImageRegionSizeType;
                        
                        ImageRegionIndexType fixedImageRegionIndex;
                        fixedImageRegionIndex[0] = FixedImageRegionOrigin.getValue()[0];
                        fixedImageRegionIndex[1] = FixedImageRegionOrigin.getValue()[1];
                        fixedImageRegionIndex[2] = FixedImageRegionOrigin.getValue()[2];
                        
                        ImageRegionSizeType fixedImageRegionSize;
                        fixedImageRegionSize[0] = FixedImageRegionSize.getValue()[0];
                        fixedImageRegionSize[1] = FixedImageRegionSize.getValue()[1];
                        fixedImageRegionSize[2] = FixedImageRegionSize.getValue()[2];
                        
                        ImageRegionType fixedImageRegion;
                        fixedImageRegion.SetIndex( fixedImageRegionIndex );
                        fixedImageRegion.SetSize( fixedImageRegionSize );
                        
                        metric->SetFixedImageRegion( fixedImageRegion );
                        
                        // Transform Parameters
                        typedef itk::Array< double > ArrayType;
                        ArrayType transformParameters( (double *) TransformParameters.getValues(0), TransformParameters.getNum(), false );
                        metric->SetTransformParameters( transformParameters );
                        
                        metric->SetComputeGradient( ComputeGradient.getValue() );
        
                        if( ComputeGradient.getValue() == TRUE )
                        {
                            metric->GetGradientImage()->Register();
                            
                            mGradientImage = new SoItkDataImage( SoItkDataImage::FLOAT, 3, 
                                FixedImage.getValue()->getModelMatrix() );
                            mGradientImage->ref();
                            mGradientImage->setPointer( metric->GetGradientImage() );
                        }
						*/
                        
                        mOutput = new SoItkDataImageMetric( SoItkDataImageMetric::UNSIGNED_SHORT, 3 );
                        mOutput->ref();
                        mOutput->setPointer( metric );
					}
					break ;
				}
			}
			break ;
		}      
    }
    catch(...)
    {
        SoDebugError::post( __FILE__, "Unknown Exception" );
        return ;
    }
    
	SO_ENGINE_OUTPUT( Output, SoItkSFDataImageMetric, setValue( mOutput ) );
//	SO_ENGINE_OUTPUT( GradientImage, SoItkSFDataImage, setValue( mGradientImage ) );
}
void QAngioSubstractionExtension::computeAutomateSingleImage()
{
    QApplication::setOverrideCursor(Qt::WaitCursor);
    const    unsigned int          Dimension = 2;
    typedef  Volume::ItkPixelType  PixelType;

    typedef itk::Image< PixelType, Dimension >  FixedImageType;
    typedef itk::Image< PixelType, Dimension >  MovingImageType;
    typedef   float     InternalPixelType;
    typedef itk::Image< InternalPixelType, Dimension > InternalImageType;

    typedef itk::TranslationTransform< double, Dimension > TransformType;
    typedef itk::GradientDescentOptimizer                  OptimizerType;
    typedef itk::LinearInterpolateImageFunction< 
                                    InternalImageType,
                                    double             > InterpolatorType;
    typedef itk::ImageRegistrationMethod< 
                                    InternalImageType, 
                                    InternalImageType >  RegistrationType;
    typedef itk::MutualInformationImageToImageMetric< 
                                          InternalImageType, 
                                          InternalImageType >    MetricType;

    TransformType::Pointer      transform     = TransformType::New();
    OptimizerType::Pointer      optimizer     = OptimizerType::New();
    InterpolatorType::Pointer   interpolator  = InterpolatorType::New();
    RegistrationType::Pointer   registration  = RegistrationType::New();

    registration->SetOptimizer(optimizer);
    registration->SetTransform(transform);
    registration->SetInterpolator(interpolator);

    MetricType::Pointer         metric        = MetricType::New();
    registration->SetMetric(metric);
    metric->SetFixedImageStandardDeviation(0.4);
    metric->SetMovingImageStandardDeviation(0.4);
    metric->SetNumberOfSpatialSamples(50);

    typedef itk::ExtractImageFilter< Volume::ItkImageType, FixedImageType > FilterType;
    
    FilterType::Pointer extractFixedImageFilter = FilterType::New();
    Volume::ItkImageType::RegionType inputRegion = m_mainVolume->getItkData()->GetLargestPossibleRegion();
    Volume::ItkImageType::SizeType size = inputRegion.GetSize();
    //Dividim la mida per dos per tal de quedar-nos només amb la part central
    // ja que si no ens registre el background
    size[0] = size[0] / 2;
    size[1] = size[1] / 2;
    size[2] = 0;
    Volume::ItkImageType::IndexType start = inputRegion.GetIndex();
    const unsigned int sliceReference = m_imageSelectorSpinBox->value();
    //comencem a un quart de la imatge
    start[0] = size[0] / 2;
    start[1] = size[1] / 2;
    start[2] = sliceReference;
    Volume::ItkImageType::RegionType desiredRegion;
    desiredRegion.SetSize(size);
    desiredRegion.SetIndex(start);
    extractFixedImageFilter->SetExtractionRegion(desiredRegion);
    extractFixedImageFilter->SetInput(m_mainVolume->getItkData());
    extractFixedImageFilter->Update();

    FilterType::Pointer extractMovingImageFilter = FilterType::New();
    Volume::ItkImageType::IndexType startMoving = inputRegion.GetIndex();
    const unsigned int sliceNumber = m_2DView_1->getViewer()->getCurrentSlice();
    startMoving[0] = size[0] / 2;
    startMoving[1] = size[1] / 2;
    startMoving[2] = sliceNumber;
    Volume::ItkImageType::RegionType desiredMovingRegion;
    desiredMovingRegion.SetSize(size);
    desiredMovingRegion.SetIndex(startMoving);
    extractMovingImageFilter->SetExtractionRegion(desiredMovingRegion);
    extractMovingImageFilter->SetInput(m_mainVolume->getItkData());
    extractMovingImageFilter->Update();

    typedef itk::NormalizeImageFilter< 
                                FixedImageType, 
                                InternalImageType 
                                        > FixedNormalizeFilterType;

    typedef itk::NormalizeImageFilter< 
                                MovingImageType, 
                                InternalImageType 
                                              > MovingNormalizeFilterType;

    FixedNormalizeFilterType::Pointer fixedNormalizer = 
                                            FixedNormalizeFilterType::New();

    MovingNormalizeFilterType::Pointer movingNormalizer =
                                            MovingNormalizeFilterType::New();
    typedef itk::DiscreteGaussianImageFilter<
                                      InternalImageType, 
                                      InternalImageType
                                                    > GaussianFilterType;

    GaussianFilterType::Pointer fixedSmoother  = GaussianFilterType::New();
    GaussianFilterType::Pointer movingSmoother = GaussianFilterType::New();

    fixedSmoother->SetVariance(2.0);
    movingSmoother->SetVariance(2.0);
    fixedNormalizer->SetInput(extractFixedImageFilter->GetOutput());
    movingNormalizer->SetInput(extractMovingImageFilter->GetOutput());

    fixedSmoother->SetInput(fixedNormalizer->GetOutput());
    movingSmoother->SetInput(movingNormalizer->GetOutput());

    registration->SetFixedImage(fixedSmoother->GetOutput());
    registration->SetMovingImage(movingSmoother->GetOutput());

    fixedNormalizer->Update();
    registration->SetFixedImageRegion(
       fixedNormalizer->GetOutput()->GetBufferedRegion());

    typedef RegistrationType::ParametersType ParametersType;
    ParametersType initialParameters(transform->GetNumberOfParameters());

    initialParameters[0] = 0.0;  // Initial offset in mm along X
    initialParameters[1] = 0.0;  // Initial offset in mm along Y

    registration->SetInitialTransformParameters(initialParameters);

    optimizer->SetLearningRate(20.0);
    optimizer->SetNumberOfIterations(200);
    optimizer->MaximizeOn();

    try 
    { 
        registration->Update();
    } 
    catch(itk::ExceptionObject & err) 
    { 
        std::cout << "ExceptionObject caught !" << std::endl; 
        std::cout << err << std::endl; 
        return;
    } 

    ParametersType finalParameters = registration->GetLastTransformParameters();

    double TranslationAlongX = finalParameters[0];
    double TranslationAlongY = finalParameters[1];

    // Print out results
    //
    DEBUG_LOG(QString("Result = "));
    DEBUG_LOG(QString(" Translation X = %1").arg(TranslationAlongX));
    DEBUG_LOG(QString(" Translation Y = %1").arg(TranslationAlongY));
    DEBUG_LOG(QString(" Iterations    = %1").arg(optimizer->GetCurrentIteration()));
    DEBUG_LOG(QString(" Metric value  = %1").arg(optimizer->GetValue()));
    double spacing[3];
    m_mainVolume->getSpacing(spacing);
    DEBUG_LOG(QString(" Translation X (in px) = %1").arg(TranslationAlongX / spacing[0]));
    DEBUG_LOG(QString(" Translation Y (in px) = %1").arg(TranslationAlongY / spacing[1]));

    //Actualitzem les dades de la transdifference tool
    m_toolManager->triggerTool("TransDifferenceTool");
    TransDifferenceTool* tdTool = static_cast<TransDifferenceTool*> (m_2DView_2->getViewer()->getToolProxy()->getTool("TransDifferenceTool"));
    if(m_tdToolData == 0){
        m_tdToolData = static_cast<TransDifferenceToolData*> (tdTool->getToolData());
    }
    if(m_tdToolData->getInputVolume() != m_mainVolume){
        m_tdToolData->setInputVolume(m_mainVolume);
    }
    tdTool->setSingleDifferenceImage(TranslationAlongX / spacing[0],TranslationAlongY / spacing[1]);
    m_toolManager->triggerTool("SlicingTool");
    

/*    typedef itk::Image< PixelType, Dimension >  FixedImageType;
    typedef itk::Image< PixelType, Dimension >  MovingImageType;
    typedef itk::TranslationTransform< double, Dimension > TransformType;
    typedef itk::RegularStepGradientDescentOptimizer       OptimizerType;
    typedef itk::MattesMutualInformationImageToImageMetric< 
                                          FixedImageType, 
                                          MovingImageType >    MetricType;
    typedef itk:: LinearInterpolateImageFunction< 
                                    MovingImageType,
                                    double          >    InterpolatorType;
    typedef itk::ImageRegistrationMethod< 
                                    FixedImageType, 
                                    MovingImageType >    RegistrationType;

    MetricType::Pointer         metric        = MetricType::New();
    TransformType::Pointer      transform     = TransformType::New();
    OptimizerType::Pointer      optimizer     = OptimizerType::New();
    InterpolatorType::Pointer   interpolator  = InterpolatorType::New();
    RegistrationType::Pointer   registration  = RegistrationType::New();

    registration->SetMetric(metric);
    registration->SetOptimizer(optimizer);
    registration->SetTransform(transform);
    registration->SetInterpolator(interpolator);

    metric->SetNumberOfHistogramBins(50);
    metric->SetNumberOfSpatialSamples(10000);

    typedef itk::ExtractImageFilter< Volume::ItkImageType, FixedImageType > FilterType;
    
    FilterType::Pointer extractFixedImageFilter = FilterType::New();
    Volume::ItkImageType::RegionType inputRegion = m_mainVolume->getItkData()->GetLargestPossibleRegion();
    Volume::ItkImageType::SizeType size = inputRegion.GetSize();
    //Dividim la mida per dos per tal de quedar-nos només amb la part central
    // ja que si no ens registre el background
    size[0] = size[0] / 2;
    size[1] = size[1] / 2;
    size[2] = 0;
    Volume::ItkImageType::IndexType start = inputRegion.GetIndex();
    const unsigned int sliceReference = m_imageSelectorSpinBox->value();
    //comencem a un quart de la imatge
    start[0] = size[0] / 2;
    start[1] = size[1] / 2;
    start[2] = sliceReference;
    Volume::ItkImageType::RegionType desiredRegion;
    desiredRegion.SetSize(size);
    desiredRegion.SetIndex(start);
    extractFixedImageFilter->SetExtractionRegion(desiredRegion);
    extractFixedImageFilter->SetInput(m_mainVolume->getItkData());
    extractFixedImageFilter->Update();

    FilterType::Pointer extractMovingImageFilter = FilterType::New();
    Volume::ItkImageType::IndexType startMoving = inputRegion.GetIndex();
    const unsigned int sliceNumber = m_2DView_1->getViewer()->getCurrentSlice();
    startMoving[0] = size[0] / 2;
    startMoving[1] = size[1] / 2;
    startMoving[2] = sliceNumber;
    Volume::ItkImageType::RegionType desiredMovingRegion;
    desiredMovingRegion.SetSize(size);
    desiredMovingRegion.SetIndex(startMoving);
    extractMovingImageFilter->SetExtractionRegion(desiredMovingRegion);
    extractMovingImageFilter->SetInput(m_mainVolume->getItkData());
    extractMovingImageFilter->Update();

    registration->SetFixedImage(extractFixedImageFilter->GetOutput());
    registration->SetMovingImage(extractMovingImageFilter->GetOutput());

    typedef RegistrationType::ParametersType ParametersType;
    ParametersType initialParameters(transform->GetNumberOfParameters());

    //Potser seria millor posar la transformada que té actualment
    initialParameters[0] = 0.0;  // Initial offset in mm along X
    initialParameters[1] = 0.0;  // Initial offset in mm along Y

    registration->SetInitialTransformParameters(initialParameters);

    optimizer->SetMaximumStepLength(4.00);  
    optimizer->SetMinimumStepLength(0.005);

    optimizer->SetNumberOfIterations(200);

    try 
    { 
        registration->StartRegistration(); 
    } 
    catch(itk::ExceptionObject & err) 
    { 
        DEBUG_LOG(QString("ExceptionObject caught !"));
        std::cout<<err<<std::endl;
        return;
    } 
    ParametersType finalParameters = registration->GetLastTransformParameters();

    const double TranslationAlongX = finalParameters[0];
    const double TranslationAlongY = finalParameters[1];

    const unsigned int numberOfIterations = optimizer->GetCurrentIteration();

    const double bestValue = optimizer->GetValue();

    DEBUG_LOG(QString("Result = "));
    DEBUG_LOG(QString(" Translation X = %1").arg(TranslationAlongX));
    DEBUG_LOG(QString(" Translation Y = %1").arg(TranslationAlongY));
    DEBUG_LOG(QString(" Iterations    = %1").arg(numberOfIterations));
    DEBUG_LOG(QString(" Metric value  = %1").arg(bestValue));

    typedef  unsigned char  OutputPixelType;
    typedef itk::Image< OutputPixelType, Dimension > OutputImageType;
    typedef itk::RescaleIntensityImageFilter< FixedImageType, FixedImageType > RescaleFilterType;
    typedef itk::ResampleImageFilter< 
                            FixedImageType, 
                            FixedImageType >    ResampleFilterType;
    typedef itk::CastImageFilter< 
                        FixedImageType,
                        OutputImageType > CastFilterType;
    typedef itk::ImageFileWriter< OutputImageType >  WriterType;

    WriterType::Pointer      writer =  WriterType::New();
    CastFilterType::Pointer  caster =  CastFilterType::New();
    ResampleFilterType::Pointer resample = ResampleFilterType::New();
    RescaleFilterType::Pointer rescaler = RescaleFilterType::New();

    rescaler->SetOutputMinimum(0);
    rescaler->SetOutputMaximum(255);

    TransformType::Pointer finalTransform = TransformType::New();
    finalTransform->SetParameters(finalParameters);
    resample->SetTransform(finalTransform);
    resample->SetSize(extractMovingImageFilter->GetOutput()->GetLargestPossibleRegion().GetSize());
    resample->SetOutputOrigin(extractMovingImageFilter->GetOutput()->GetOrigin());
    resample->SetOutputSpacing(extractMovingImageFilter->GetOutput()->GetSpacing());
    resample->SetDefaultPixelValue(100);

    writer->SetFileName("prova.jpg");

    rescaler->SetInput(extractMovingImageFilter->GetOutput());
    resample->SetInput(rescaler->GetOutput());
    caster->SetInput(resample->GetOutput());
    writer->SetInput(caster->GetOutput());
    writer->Update();
*/

    QApplication::restoreOverrideCursor();

}