double antsImageMIHelper(
  typename itk::Image< float , Dimension >::Pointer image1,
  typename itk::Image< float , Dimension >::Pointer image2 )
{
  typedef itk::Image< float , Dimension > ImageType;
  if( image1.IsNotNull() & image2.IsNotNull() )
  {
    double mi = 1;
    typedef itk::MattesMutualInformationImageToImageMetricv4
      <ImageType, ImageType, ImageType> MetricType;
    unsigned int bins = 32;
    typename MetricType::Pointer metric = MetricType::New();
    metric->SetFixedImage( image1 );
    metric->SetMovingImage( image2 );
    metric->SetNumberOfHistogramBins( bins );
    metric->Initialize();
    mi = metric->GetValue();
    return mi;
  }
  else
  {
    return 1;
  }
}
template<class FixedImageType,class MovingImageType,class TransformType> void RegisterPair(const typename FixedImageType::Pointer & fixedImage, const typename MovingImageType::Pointer & movingImage, typename TransformType::Pointer & transform ){


	typedef itk::LogImageAdaptor<FixedImageType,float> LogFixedImageType;

	typedef itk::CenteredTransformInitializer< TransformType,LogFixedImageType,MovingImageType> TransformInitializerType;
	typename TransformInitializerType::Pointer initializer = TransformInitializerType::New();

	typename LogFixedImageType::Pointer logAdaptor=LogFixedImageType::New();
	logAdaptor->SetImage(fixedImage);

	initializer->SetTransform( transform );
	initializer->SetFixedImage( logAdaptor );
	initializer->SetMovingImage( movingImage);

	initializer->MomentsOn();
	initializer->InitializeTransform();

	  // Optimizer Type
	  typedef itk::VersorRigid3DTransformOptimizer OptimizerType;
	 //typedef itk::ConjugateGradientOptimizer OptimizerType;
	  // Metric Type
	  typedef
	  //itk::NormalizedMutualInformationHistogramImageToImageMetric< FixedImageType, MovingImageType >

	  //itk::MutualInformationImageToImageMetric<FixedImageType,MovingImageType >
			  //itk::GradientDifferenceImageToImageMetric<FixedImageType,MovingImageType>
			  itk::NormalizedCorrelationImageToImageMetric< FixedImageType, MovingImageType >
	//		  itk::KullbackLeiblerCompareHistogramImageToImageMetric< FixedImageType, MovingImageType >
			  //itk::MattesMutualInformationImageToImageMetric<FixedImageType,MovingImageType>
			  //itk::MutualInformationHistogramImageToImageMetric<FixedImageType,MovingImageType>
			  //itk::MeanSquaresImageToImageMetric< FixedImageType, MovingImageType >
	  MetricType;


	  // Interpolation technique
	  typedef itk:: LinearInterpolateImageFunction<
	                                    MovingImageType,
	                                    double          >    InterpolatorType;

	  // Registration Method
	  typedef itk::ImageRegistrationMethod<
	                                    FixedImageType,
	                                    MovingImageType >    RegistrationType;


	  typename MetricType::Pointer         metric        = MetricType::New();

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

  /******************************************************************
   * Set up the optimizer.
   ******************************************************************/
	  typedef typename TransformType::VersorType VersorType;
	  typedef typename VersorType::VectorType VectorType;
	  VersorType rotation;
	  VectorType axis;
	  	axis[0] = 0.0;
	  	axis[1] = 0.0;
	  	axis[2] = 1.0;
	  	const double angle = 0;
	  	rotation.Set( axis, angle );
	  	transform->SetRotation( rotation );

	  	registration->SetInitialTransformParameters( transform->GetParameters() );
	  	// Software Guide : EndCodeSnippet
	  	typedef OptimizerType::ScalesType OptimizerScalesType;
	  	OptimizerScalesType optimizerScales( transform->GetNumberOfParameters() );
	  	const double translationScale = 1.0 / 1000.0;
	  	optimizerScales[0] = 1.0;
	  	optimizerScales[1] = 1.0;
	  	optimizerScales[2] = 1.0;
	  	optimizerScales[3] = translationScale;
	  	optimizerScales[4] = translationScale;
	  	optimizerScales[5] = translationScale;
	  	optimizer->SetScales( optimizerScales );
	  	//optimizer->SetMaximumStepLength( 0.1 );
	  	optimizer->SetMaximumStepLength( 1);
	  	optimizer->SetMinimumStepLength( 0.000000001 );
	  	optimizer->SetNumberOfIterations( 10 );
	  	optimizer->SetMinimize(true);



  /******************************************************************
   * Set up the metric.
   ******************************************************************/
  //metric->SetMovingImageStandardDeviation( 1.0 );
  //metric->SetFixedImageStandardDeviation( 1.0 );

  //metric->SetNumberOfSpatialSamples( 50000000 );
  metric->SetUseAllPixels(true);
  //metric->SetFixedImageRegion( fixedImage->GetBufferedRegion() );

  /******************************************************************
   * Set up the registrator.
   ******************************************************************/

  // connect up the components
  registration->SetMetric( metric );
  registration->SetOptimizer( optimizer );
  registration->SetTransform( transform );
  registration->SetFixedImage( fixedImage );
  registration->SetMovingImage( movingImage );
  registration->SetInterpolator( interpolator );

//  transform->SetIdentity();
//  // set initial parameters to identity
//  typename RegistrationType::ParametersType initialParameters(
//    transform->GetNumberOfParameters() );
//
//  initialParameters.Fill( 0.0 );
//  //initialParameters[3] = 1.0;


	CommandIterationUpdate::Pointer observer = CommandIterationUpdate::New();
	optimizer->AddObserver( itk::IterationEvent(), observer );
	try
	{
	registration->Update();
	std::cout << "Optimizer stop condition: "
	<< registration->GetOptimizer()->GetStopConditionDescription()
	<< std::endl;
	}
	catch( itk::ExceptionObject & err )
	{
	std::cerr << "ExceptionObject caught !" << std::endl;
	std::cerr << err << std::endl;
	//return EXIT_FAILURE;
	}
	OptimizerType::ParametersType finalParameters =
	registration->GetLastTransformParameters();
	const double versorX = finalParameters[0];
	const double versorY = finalParameters[1];
	const double versorZ = finalParameters[2];
	const double finalTranslationX = finalParameters[3];
	const double finalTranslationY = finalParameters[4];
	const double finalTranslationZ = finalParameters[5];
	const unsigned int numberOfIterations = optimizer->GetCurrentIteration();
	const double bestValue = optimizer->GetValue();
	// Print out results
	//
	std::cout << std::endl << std::endl;
	std::cout << "Result = " << std::endl;
	std::cout << " versor X = " << versorX << std::endl;
	std::cout << " versor Y = " << versorY << std::endl;
	std::cout << " versor Z = " << versorZ << std::endl;
	std::cout << " Translation X = " << finalTranslationX << std::endl;
	std::cout << " Translation Y = " << finalTranslationY << std::endl;
	std::cout << " Translation Z = " << finalTranslationZ << std::endl;
	std::cout << " Iterations = " << numberOfIterations << std::endl;
	std::cout << " Metric value = " << bestValue << std::endl;



}
    void BSplineRegistration::GenerateData2( itk::Image<TPixel, VImageDimension>* itkImage1)
  {
    std::cout << "start bspline registration" << std::endl;
    
    // Typedefs
    typedef typename itk::Image< TPixel, VImageDimension >  InternalImageType;
       
    typedef typename itk::Vector< float, VImageDimension >    VectorPixelType;
    typedef typename itk::Image<  VectorPixelType, VImageDimension > DeformationFieldType;

    typedef itk::BSplineDeformableTransform<
                                double,
                                VImageDimension,
                                3 >                         TransformType;

    typedef typename TransformType::ParametersType          ParametersType;



    
    //typedef itk::LBFGSOptimizer                             OptimizerType;
    typedef itk::SingleValuedNonLinearOptimizer             OptimizerType;
    //typedef itk::SingleValuedCostFunction                   MetricType;

    typedef itk::MattesMutualInformationImageToImageMetric<
                                InternalImageType,
                                InternalImageType >           MetricType;

    typedef itk::MeanSquaresImageToImageMetric<
                                InternalImageType,
                                InternalImageType >           MetricTypeMS;

    typedef itk::LinearInterpolateImageFunction<
                                InternalImageType,
                                double >                    InterpolatorType;

    typedef itk::ImageRegistrationMethod<
                                InternalImageType,
                                InternalImageType >           RegistrationType;

    typedef typename itk::WarpImageFilter<
                            InternalImageType, 
                            InternalImageType,
                            DeformationFieldType  >         WarperType;

    typedef typename TransformType::SpacingType                      SpacingType;

    typedef typename TransformType::OriginType                       OriginType;

    typedef itk::ResampleImageFilter< 
                                InternalImageType, 
                                InternalImageType >            ResampleFilterType;

    typedef itk::Image< TPixel, VImageDimension >           OutputImageType;
  

    // Sample new image with the same image type as the fixed image
    typedef itk::CastImageFilter< 
                                InternalImageType,
                                InternalImageType >            CastFilterType;
                    
    
    typedef itk::Vector< float, VImageDimension >           VectorType;
    typedef itk::Image< VectorType, VImageDimension >       DeformationFieldType;


    typedef itk::BSplineDeformableTransformInitializer <
                                TransformType,
                                InternalImageType >            InitializerType;
    

    typename InterpolatorType::Pointer   interpolator  = InterpolatorType::New();
    typename RegistrationType::Pointer   registration  = RegistrationType::New();   
    typename InitializerType::Pointer    initializer   = InitializerType::New();
    typename TransformType::Pointer      transform     = TransformType::New();
    
    
    if(m_Metric==0 || m_Metric==1)
    {
      typename MetricType::Pointer metric = MetricType::New();
      metric->SetNumberOfHistogramBins( 32);
      metric->SetNumberOfSpatialSamples(90000);
      registration->SetMetric(          metric       );
    }
    else{
      typename MetricTypeMS::Pointer metric = MetricTypeMS::New();  
      registration->SetMetric(          metric       );
    }
     
    typename OptimizerFactory::Pointer optFac = OptimizerFactory::New();
    optFac->SetOptimizerParameters(m_OptimizerParameters);
    optFac->SetNumberOfTransformParameters(transform->GetNumberOfParameters());
    OptimizerType::Pointer optimizer = optFac->GetOptimizer();    

    optimizer->AddObserver(itk::AnyEvent(), m_Observer);
    
    //typedef mitk::MetricFactory <TPixel, VImageDimension> MetricFactoryType;
    //typename MetricFactoryType::Pointer metricFac = MetricFactoryType::New();
    //metricFac->SetMetricParameters(m_MetricParameters);
    ////MetricType::Pointer metric = metricFac->GetMetric();


    typename InternalImageType::Pointer fixedImage = InternalImageType::New();
    mitk::CastToItkImage(m_ReferenceImage, fixedImage);    
    typename InternalImageType::Pointer movingImage = itkImage1;
    typename InternalImageType::RegionType fixedRegion = fixedImage->GetBufferedRegion();
    typename InternalImageType::RegionType movingRegion = movingImage->GetBufferedRegion();

    
    if(m_MatchHistograms)
    {
      typedef itk::RescaleIntensityImageFilter<InternalImageType,InternalImageType> FilterType;   
      typedef itk::HistogramMatchingImageFilter<InternalImageType,InternalImageType> HEFilterType;

      typename FilterType::Pointer inputRescaleFilter = FilterType::New();  
      typename FilterType::Pointer referenceRescaleFilter = FilterType::New();  

      referenceRescaleFilter->SetInput(fixedImage);
      inputRescaleFilter->SetInput(movingImage);

      TPixel desiredMinimum =  0;
      TPixel desiredMaximum =  255;
      
      referenceRescaleFilter->SetOutputMinimum( desiredMinimum );
      referenceRescaleFilter->SetOutputMaximum( desiredMaximum );
      referenceRescaleFilter->UpdateLargestPossibleRegion();  
      inputRescaleFilter->SetOutputMinimum( desiredMinimum );
      inputRescaleFilter->SetOutputMaximum( desiredMaximum );
      inputRescaleFilter->UpdateLargestPossibleRegion();

      // Histogram match the images
      typename HEFilterType::Pointer intensityEqualizeFilter = HEFilterType::New();

      intensityEqualizeFilter->SetReferenceImage( inputRescaleFilter->GetOutput() );
      intensityEqualizeFilter->SetInput( referenceRescaleFilter->GetOutput() );
      intensityEqualizeFilter->SetNumberOfHistogramLevels( 64 );
      intensityEqualizeFilter->SetNumberOfMatchPoints( 12 );
      intensityEqualizeFilter->ThresholdAtMeanIntensityOn();
      intensityEqualizeFilter->Update();

      //fixedImage = referenceRescaleFilter->GetOutput();
      //movingImage = IntensityEqualizeFilter->GetOutput();

      fixedImage = intensityEqualizeFilter->GetOutput();
      movingImage = inputRescaleFilter->GetOutput();
    }


    //
    registration->SetOptimizer(       optimizer     );
    registration->SetInterpolator(    interpolator  );  
    registration->SetFixedImage(      fixedImage    );
    registration->SetMovingImage(     movingImage   );    
    registration->SetFixedImageRegion(fixedRegion   );

    initializer->SetTransform(transform);
    initializer->SetImage(fixedImage);
    initializer->SetNumberOfGridNodesInsideTheImage( m_NumberOfGridPoints );
    initializer->InitializeTransform();

    registration->SetTransform( transform );    

    const unsigned int numberOfParameters = transform->GetNumberOfParameters();
    
    typename itk::BSplineDeformableTransform<
                                double,
                                VImageDimension,
                                3 >::ParametersType  parameters;

    parameters.set_size(numberOfParameters);
    parameters.Fill( 0.0 );
    transform->SetParameters( parameters );

    // We now pass the parameters of the current transform as the initial
    // parameters to be used when the registration process starts.
    registration->SetInitialTransformParameters( transform->GetParameters() );
    
    std::cout << "Intial Parameters = " << std::endl;
    std::cout << transform->GetParameters() << std::endl;
 

    std::cout << std::endl << "Starting Registration" << std::endl;

    try 
    { 
      double tstart(clock());     
      registration->StartRegistration();    
      double time = clock() - tstart;
      time = time / CLOCKS_PER_SEC;
      MITK_INFO << "Registration time: " << time;
    } 
    catch( itk::ExceptionObject & err ) 
    { 
      std::cerr << "ExceptionObject caught !" << std::endl; 
      std::cerr << err << std::endl;       
    } 
    
    typename OptimizerType::ParametersType finalParameters = 
                      registration->GetLastTransformParameters();

    std::cout << "Last Transform Parameters" << std::endl;
    std::cout << finalParameters << std::endl;

    transform->SetParameters( finalParameters );

/*
    ResampleFilterType::Pointer       resampler = ResampleFilterType::New();
    resampler->SetTransform(          transform );
    resampler->SetInput(              movingImage );
    resampler->SetSize(               fixedImage->GetLargestPossibleRegion().GetSize() );
    resampler->SetOutputOrigin(       fixedImage->GetOrigin() );
    resampler->SetOutputSpacing(      fixedImage->GetSpacing() );
    resampler->SetOutputDirection(    fixedImage->GetDirection() );
    resampler->SetDefaultPixelValue(  100 );
    resampler->SetInterpolator(       interpolator);
    resampler->Update();*/



    // Generate deformation field
    typename DeformationFieldType::Pointer field = DeformationFieldType::New();    
    field->SetRegions( movingRegion );
    field->SetOrigin( movingImage->GetOrigin() );
    field->SetSpacing( movingImage->GetSpacing() );
    field->SetDirection( movingImage->GetDirection() );   
    field->Allocate();


    typedef itk::ImageRegionIterator< DeformationFieldType > FieldIterator;
    FieldIterator fi( field, movingRegion );
    fi.GoToBegin();

    typename TransformType::InputPointType  fixedPoint;
    typename TransformType::OutputPointType movingPoint;
    typename DeformationFieldType::IndexType index;

    VectorType displacement;

    while( ! fi.IsAtEnd() )
    {
      index = fi.GetIndex();
      field->TransformIndexToPhysicalPoint( index, fixedPoint );
      movingPoint = transform->TransformPoint( fixedPoint );
      displacement = movingPoint - fixedPoint;
      fi.Set( displacement );
      ++fi;
    }


    // Use the deformation field to warp the moving image
    typename WarperType::Pointer warper = WarperType::New();    
    warper->SetInput( movingImage );
    warper->SetInterpolator( interpolator );
    warper->SetOutputSpacing( movingImage->GetSpacing() );
    warper->SetOutputOrigin( movingImage->GetOrigin() );
    warper->SetOutputDirection( movingImage->GetDirection() );
    warper->SetDeformationField( field );
    warper->Update();

    typename InternalImageType::Pointer result = warper->GetOutput();    

    if(m_UpdateInputImage)
    {   
      Image::Pointer outputImage = this->GetOutput();
      mitk::CastToMitkImage( result, outputImage );
    }


    // Save the deformationfield resulting from the registration    
    if(m_SaveDeformationField)
    {      
      typedef itk::ImageFileWriter< DeformationFieldType >  FieldWriterType;
      typename FieldWriterType::Pointer fieldWriter = FieldWriterType::New();

      fieldWriter->SetInput( field );
      
      fieldWriter->SetFileName( m_DeformationFileName );
      try
      {
        fieldWriter->Update();
      }
      catch( itk::ExceptionObject & excp )
      {
        std::cerr << "Exception thrown " << std::endl;
        std::cerr << excp << std::endl;
        //return EXIT_FAILURE;
      }
    }



  }
SEXP invariantSimilarityHelper(
  typename itk::Image< float , ImageDimension >::Pointer image1,
  typename itk::Image< float , ImageDimension >::Pointer image2,
  SEXP r_thetas, SEXP r_lsits, SEXP r_WM, SEXP r_scale,
  SEXP r_doreflection, SEXP r_txfn  )
{
  unsigned int mibins = 20;
  unsigned int localSearchIterations =
    Rcpp::as< unsigned int >( r_lsits ) ;
  std::string whichMetric = Rcpp::as< std::string >( r_WM );
  std::string txfn = Rcpp::as< std::string >( r_txfn );
  bool useprincaxis = true;
  typedef typename itk::ImageMaskSpatialObject<ImageDimension>::ImageType
    maskimagetype;
  typename maskimagetype::Pointer mask = ITK_NULLPTR;
  Rcpp::NumericVector thetas( r_thetas );
  Rcpp::NumericVector vector_r( r_thetas ) ;
  Rcpp::IntegerVector dims( 1 );
  Rcpp::IntegerVector doReflection( r_doreflection );
  unsigned int vecsize = thetas.size();
  dims[0]=0;
  typedef float  PixelType;
  typedef double RealType;
  RealType bestscale = Rcpp::as< RealType >( r_scale ) ;
  typedef itk::Image< PixelType , ImageDimension > ImageType;
  if( image1.IsNotNull() & image2.IsNotNull() )
    {
    typedef typename itk::ImageMomentsCalculator<ImageType> ImageCalculatorType;
    typedef itk::AffineTransform<RealType, ImageDimension> AffineType0;
    typedef itk::AffineTransform<RealType, ImageDimension> AffineType;
    typedef typename ImageCalculatorType::MatrixType       MatrixType;
    typedef itk::Vector<float, ImageDimension>  VectorType;
    VectorType ccg1;
    VectorType cpm1;
    MatrixType cpa1;
    VectorType ccg2;
    VectorType cpm2;
    MatrixType cpa2;
    typename ImageCalculatorType::Pointer calculator1 =
      ImageCalculatorType::New();
    typename ImageCalculatorType::Pointer calculator2 =
      ImageCalculatorType::New();
    calculator1->SetImage(  image1 );
    calculator2->SetImage(  image2 );
    typename ImageCalculatorType::VectorType fixed_center;
    fixed_center.Fill(0);
    typename ImageCalculatorType::VectorType moving_center;
    moving_center.Fill(0);
    try
      {
      calculator1->Compute();
      fixed_center = calculator1->GetCenterOfGravity();
      ccg1 = calculator1->GetCenterOfGravity();
      cpm1 = calculator1->GetPrincipalMoments();
      cpa1 = calculator1->GetPrincipalAxes();
      try
        {
        calculator2->Compute();
        moving_center = calculator2->GetCenterOfGravity();
        ccg2 = calculator2->GetCenterOfGravity();
        cpm2 = calculator2->GetPrincipalMoments();
        cpa2 = calculator2->GetPrincipalAxes();
        }
      catch( ... )
        {
        fixed_center.Fill(0);
        }
      }
    catch( ... )
      {
      // Rcpp::Rcerr << " zero image1 error ";
      }
    if ( vnl_math_abs( bestscale - 1.0 ) < 1.e-6 )
      {
      RealType volelt1 = 1;
      RealType volelt2 = 1;
      for ( unsigned int d=0; d<ImageDimension; d++)
        {
        volelt1 *= image1->GetSpacing()[d];
        volelt2 *= image2->GetSpacing()[d];
        }
      bestscale =
        ( calculator2->GetTotalMass() * volelt2 )/
        ( calculator1->GetTotalMass() * volelt1 );
      RealType powlev = 1.0 / static_cast<RealType>(ImageDimension);
      bestscale = vcl_pow( bestscale , powlev );
    }
    unsigned int eigind1 = 1;
    unsigned int eigind2 = 1;
    if( ImageDimension == 3 )
      {
      eigind1 = 2;
      }
    typedef vnl_vector<RealType> EVectorType;
    typedef vnl_matrix<RealType> EMatrixType;
    EVectorType evec1_2ndary = cpa1.GetVnlMatrix().get_row( eigind2 );
    EVectorType evec1_primary = cpa1.GetVnlMatrix().get_row( eigind1 );
    EVectorType evec2_2ndary  = cpa2.GetVnlMatrix().get_row( eigind2 );
    EVectorType evec2_primary = cpa2.GetVnlMatrix().get_row( eigind1 );
    /** Solve Wahba's problem http://en.wikipedia.org/wiki/Wahba%27s_problem */
    EMatrixType B = outer_product( evec2_primary, evec1_primary );
    if( ImageDimension == 3 )
      {
      B = outer_product( evec2_2ndary, evec1_2ndary )
        + outer_product( evec2_primary, evec1_primary );
      }
    vnl_svd<RealType>    wahba( B );
    vnl_matrix<RealType> A_solution = wahba.V() * wahba.U().transpose();
    A_solution = vnl_inverse( A_solution );
    RealType det = vnl_determinant( A_solution  );
    if( ( det < 0 ) )
      {
      vnl_matrix<RealType> id( A_solution );
      id.set_identity();
      for( unsigned int i = 0; i < ImageDimension; i++ )
        {
        if( A_solution( i, i ) < 0 )
          {
          id( i, i ) = -1.0;
          }
        }
      A_solution =  A_solution * id.transpose();
      }
    if ( doReflection[0] == 1 ||  doReflection[0] == 3 )
      {
        vnl_matrix<RealType> id( A_solution );
        id.set_identity();
        id = id - 2.0 * outer_product( evec2_primary , evec2_primary  );
        A_solution = A_solution * id;
      }
    if ( doReflection[0] > 1 )
      {
        vnl_matrix<RealType> id( A_solution );
        id.set_identity();
        id = id - 2.0 * outer_product( evec1_primary , evec1_primary  );
        A_solution = A_solution * id;
      }
    typename AffineType::Pointer affine1 = AffineType::New();
    typename AffineType::OffsetType trans = affine1->GetOffset();
    itk::Point<RealType, ImageDimension> trans2;
    for( unsigned int i = 0; i < ImageDimension; i++ )
      {
      trans[i] = moving_center[i] - fixed_center[i];
      trans2[i] =  fixed_center[i] * ( 1 );
      }
    affine1->SetIdentity();
    affine1->SetOffset( trans );
    if( useprincaxis )
      {
      affine1->SetMatrix( A_solution );
      }
    affine1->SetCenter( trans2 );
    if( ImageDimension > 3  )
      {
      return EXIT_SUCCESS;
      }
    vnl_vector<RealType> evec_tert;
    if( ImageDimension == 3 )
      { // try to rotate around tertiary and secondary axis
      evec_tert = vnl_cross_3d( evec1_primary, evec1_2ndary );
      }
    if( ImageDimension == 2 )
      { // try to rotate around tertiary and secondary axis
      evec_tert = evec1_2ndary;
      evec1_2ndary = evec1_primary;
      }
    itk::Vector<RealType, ImageDimension> axis2;
    itk::Vector<RealType, ImageDimension> axis1;
    for( unsigned int d = 0; d < ImageDimension; d++ )
      {
      axis1[d] = evec_tert[d];
      axis2[d] = evec1_2ndary[d];
      }
    typename AffineType::Pointer simmer = AffineType::New();
    simmer->SetIdentity();
    simmer->SetCenter( trans2 );
    simmer->SetOffset( trans );
    typename AffineType0::Pointer affinesearch = AffineType0::New();
    affinesearch->SetIdentity();
    affinesearch->SetCenter( trans2 );
    typedef  itk::MultiStartOptimizerv4         OptimizerType;
    typename OptimizerType::MetricValuesListType metricvalues;
    typename OptimizerType::Pointer  mstartOptimizer = OptimizerType::New();
    typedef itk::CorrelationImageToImageMetricv4
      <ImageType, ImageType, ImageType> GCMetricType;
    typedef itk::MattesMutualInformationImageToImageMetricv4
      <ImageType, ImageType, ImageType> MetricType;
    typename MetricType::ParametersType newparams(  affine1->GetParameters() );
    typename GCMetricType::Pointer gcmetric = GCMetricType::New();
    gcmetric->SetFixedImage( image1 );
    gcmetric->SetVirtualDomainFromImage( image1 );
    gcmetric->SetMovingImage( image2 );
    gcmetric->SetMovingTransform( simmer );
    gcmetric->SetParameters( newparams );
    typename MetricType::Pointer mimetric = MetricType::New();
    mimetric->SetNumberOfHistogramBins( mibins );
    mimetric->SetFixedImage( image1 );
    mimetric->SetMovingImage( image2 );
    mimetric->SetMovingTransform( simmer );
    mimetric->SetParameters( newparams );
    if( mask.IsNotNull() )
      {
      typename itk::ImageMaskSpatialObject<ImageDimension>::Pointer so =
        itk::ImageMaskSpatialObject<ImageDimension>::New();
      so->SetImage( const_cast<maskimagetype *>( mask.GetPointer() ) );
      mimetric->SetFixedImageMask( so );
      gcmetric->SetFixedImageMask( so );
      }
    typedef  itk::ConjugateGradientLineSearchOptimizerv4 LocalOptimizerType;
    typename LocalOptimizerType::Pointer  localoptimizer =
      LocalOptimizerType::New();
    RealType     localoptimizerlearningrate = 0.1;
    localoptimizer->SetLearningRate( localoptimizerlearningrate );
    localoptimizer->SetMaximumStepSizeInPhysicalUnits(
      localoptimizerlearningrate );
    localoptimizer->SetNumberOfIterations( localSearchIterations );
    localoptimizer->SetLowerLimit( 0 );
    localoptimizer->SetUpperLimit( 2 );
    localoptimizer->SetEpsilon( 0.1 );
    localoptimizer->SetMaximumLineSearchIterations( 50 );
    localoptimizer->SetDoEstimateLearningRateOnce( true );
    localoptimizer->SetMinimumConvergenceValue( 1.e-6 );
    localoptimizer->SetConvergenceWindowSize( 5 );
    if( true )
      {
      typedef typename MetricType::FixedSampledPointSetType PointSetType;
      typedef typename PointSetType::PointType              PointType;
      typename PointSetType::Pointer      pset(PointSetType::New());
      unsigned int ind=0;
      unsigned int ct=0;
      itk::ImageRegionIteratorWithIndex<ImageType> It(image1,
        image1->GetLargestPossibleRegion() );
      for( It.GoToBegin(); !It.IsAtEnd(); ++It )
        {
        // take every N^th point
        if ( ct % 10 == 0  )
          {
          PointType pt;
          image1->TransformIndexToPhysicalPoint( It.GetIndex(), pt);
          pset->SetPoint(ind, pt);
          ind++;
          }
          ct++;
        }
      mimetric->SetFixedSampledPointSet( pset );
      mimetric->SetUseFixedSampledPointSet( true );
      gcmetric->SetFixedSampledPointSet( pset );
      gcmetric->SetUseFixedSampledPointSet( true );
    }
    if ( whichMetric.compare("MI") == 0  ) {
      mimetric->Initialize();
      typedef itk::RegistrationParameterScalesFromPhysicalShift<MetricType>
      RegistrationParameterScalesFromPhysicalShiftType;
      typename RegistrationParameterScalesFromPhysicalShiftType::Pointer
      shiftScaleEstimator =
      RegistrationParameterScalesFromPhysicalShiftType::New();
      shiftScaleEstimator->SetMetric( mimetric );
      shiftScaleEstimator->SetTransformForward( true );
      typename RegistrationParameterScalesFromPhysicalShiftType::ScalesType
      movingScales( simmer->GetNumberOfParameters() );
      shiftScaleEstimator->EstimateScales( movingScales );
      mstartOptimizer->SetScales( movingScales );
      mstartOptimizer->SetMetric( mimetric );
      localoptimizer->SetMetric( mimetric );
      localoptimizer->SetScales( movingScales );
    }
    if ( whichMetric.compare("MI") != 0  ) {
      gcmetric->Initialize();
      typedef itk::RegistrationParameterScalesFromPhysicalShift<GCMetricType>
        RegistrationParameterScalesFromPhysicalShiftType;
      typename RegistrationParameterScalesFromPhysicalShiftType::Pointer
        shiftScaleEstimator =
        RegistrationParameterScalesFromPhysicalShiftType::New();
      shiftScaleEstimator->SetMetric( gcmetric );
      shiftScaleEstimator->SetTransformForward( true );
      typename RegistrationParameterScalesFromPhysicalShiftType::ScalesType
      movingScales( simmer->GetNumberOfParameters() );
      shiftScaleEstimator->EstimateScales( movingScales );
      mstartOptimizer->SetScales( movingScales );
      mstartOptimizer->SetMetric( gcmetric );
      localoptimizer->SetMetric( gcmetric );
      localoptimizer->SetScales( movingScales );
    }
    typename OptimizerType::ParametersListType parametersList =
      mstartOptimizer->GetParametersList();
    affinesearch->SetIdentity();
    affinesearch->SetCenter( trans2 );
    affinesearch->SetOffset( trans );
    for ( unsigned int i = 0; i < vecsize; i++ )
      {
      RealType ang1 = thetas[i];
      RealType ang2 = 0; // FIXME should be psi
      vector_r[ i ]=0;
      if( ImageDimension == 3 )
        {
        for ( unsigned int jj = 0; jj < vecsize; jj++ )
        {
        ang2=thetas[jj];
        affinesearch->SetIdentity();
        affinesearch->SetCenter( trans2 );
        affinesearch->SetOffset( trans );
        if( useprincaxis )
          {
          affinesearch->SetMatrix( A_solution );
          }
        affinesearch->Rotate3D(axis1, ang1, 1);
        affinesearch->Rotate3D(axis2, ang2, 1);
        affinesearch->Scale( bestscale );
        simmer->SetMatrix(  affinesearch->GetMatrix() );
        parametersList.push_back( simmer->GetParameters() );
        }
        }
      if( ImageDimension == 2 )
        {
        affinesearch->SetIdentity();
        affinesearch->SetCenter( trans2 );
        affinesearch->SetOffset( trans );
        if( useprincaxis )
          {
          affinesearch->SetMatrix( A_solution );
          }
        affinesearch->Rotate2D( ang1, 1);
        affinesearch->Scale( bestscale );
        simmer->SetMatrix(  affinesearch->GetMatrix() );
        typename AffineType::ParametersType pp =
          simmer->GetParameters();
        //pp[1]=ang1;
        //pp[0]=bestscale;
        parametersList.push_back( simmer->GetParameters() );
        }
      }
    mstartOptimizer->SetParametersList( parametersList );
    if( localSearchIterations > 0 )
      {
      mstartOptimizer->SetLocalOptimizer( localoptimizer );
      }
    mstartOptimizer->StartOptimization();
    typename AffineType::Pointer bestaffine = AffineType::New();
    bestaffine->SetCenter( trans2 );
    bestaffine->SetParameters( mstartOptimizer->GetBestParameters() );
    if ( txfn.length() > 3 )
      {
      typename AffineType::Pointer bestaffine = AffineType::New();
      bestaffine->SetCenter( trans2 );
      bestaffine->SetParameters( mstartOptimizer->GetBestParameters() );
      typedef itk::TransformFileWriter TransformWriterType;
      typename TransformWriterType::Pointer transformWriter =
        TransformWriterType::New();
      transformWriter->SetInput( bestaffine );
      transformWriter->SetFileName( txfn.c_str() );
      transformWriter->Update();
      }
    metricvalues = mstartOptimizer->GetMetricValuesList();
    for ( unsigned int k = 0; k < metricvalues.size(); k++ )
      {
      vector_r[k] = metricvalues[k];
      }
    dims[0] = vecsize;
    vector_r.attr( "dim" ) = vecsize;
    return Rcpp::wrap( vector_r );
    }
  else
    {
    return Rcpp::wrap( vector_r );
    }
}