Ejemplo n.º 1
0
void DeepCopy(BigMatrix *pInMat, BigMatrix *pOutMat, SEXP rowInds, SEXP colInds)
{
    in_BMAccessorType inMat( *pInMat );
    out_BMAccessorType outMat( *pOutMat );

    double *pRows = NUMERIC_DATA(rowInds);
    double *pCols = NUMERIC_DATA(colInds);
    index_type nRows = GET_LENGTH(rowInds);
    index_type nCols = GET_LENGTH(colInds);

    if (nRows != pOutMat->nrow())
        Rf_error("length of row indices does not equal # of rows in new matrix");
    if (nCols != pOutMat->ncol())
        Rf_error("length of col indices does not equal # of cols in new matrix");

    index_type i = 0;
    index_type j = 0;
    in_CType *pInColumn;
    out_CType *pOutColumn;

    for (i = 0; i < nCols; ++i) {
        pInColumn = inMat[static_cast<index_type>(pCols[i])-1];
        pOutColumn = outMat[i];
        for (j = 0; j < nRows; ++j) {
            pOutColumn[j] = static_cast<out_CType>(
                                pInColumn[static_cast<index_type>(pRows[j])-1]);
        }
    }

    return;
}
Ejemplo n.º 2
0
/**
   Returns the pixels in the actual image which fall insdie
   the contour.
   * @param:
   *   contour - vector of points which form a contour
   *   src     - source Mat from which the pixels need to be extracted.
   *   dst     - the output matrix in which the contour pixels are saved. */
void getContourPixels(std::vector <cv::Point> contour,
		      cv::Mat &src, cv::Mat &dst) {
  cv::Rect R = cv::boundingRect(contour);
  cv::Mat outMat(cv::Size(R.width, R.height), src.type());
  outMat = src(cv::Range(R.y, R.y + R.height),
	       cv::Range(R.x, R.x + R.width));
  outMat.copyTo(dst);
}
Ejemplo n.º 3
0
bool TextureIO::saveTexture(const string &imgPath, const Texture &texture, bool flip)
{
    Mat outMat(texture.getHeight(), texture.getWidth(), CV_8UC4);
    texture.bind(); 
    glGetTexImage( GL_TEXTURE_2D, 0, GL_BGRA, GL_UNSIGNED_BYTE, outMat.data );
    
    if(flip){cv::flip(outMat, outMat, 0);}
    return imwrite(imgPath, outMat);
}
Ejemplo n.º 4
0
void SkyDetect::createPicBuffer( unsigned int*& imgBuffer )
{
	// create ARGB 4x8bits picture
	cv::Mat alpha(cv::Size( mWidth, mHeight), CV_8UC1);
	cv::Mat outMat(cv::Size( mWidth, mHeight),CV_8UC4);
	cv::Mat in[] = {  alpha, mImageIn };
	int from_to[] = { 0,3,  1,0,  2,1,  3,2 };
	cv::mixChannels( in, 2, &outMat, 1, from_to, 4 );

	//qDebug() << "type2: " << outMat.type();
	//cv::imshow( "Input Image2", outMat );


	imgBuffer = new UINT[mSize];
	memcpy((void*) imgBuffer, outMat.data, mSize*sizeof(UINT) );
}
Ejemplo n.º 5
0
cv::Mat cropRect(const cv::Mat & image, const C2dImagePointPx & top1, const C2dImagePointPx & top2, const C2dImagePointPx & bottom2, const C2dImagePointPx & bottom1, const int nOutputRows, const int nOutputCols)
{

    cv::Mat outMat(nOutputRows, nOutputCols, CV_8UC3);

    for(int r=0; r<nOutputRows; r++) {
        C2dImagePointPx rowStart = (bottom1 + (bottom2-bottom1)*(r/(double)nOutputRows)).eval();
        C2dImagePointPx rowEnd = (top1 + (top2-top1)*(r/(double)nOutputRows)).eval();
        for(int c=0; c<nOutputCols; c++) {
            C2dImagePointPx rowPos = (rowStart + (rowEnd-rowStart)*(c/(double)nOutputCols)).eval();
            outMat.at<cv::Vec3b>(r, c) = getSubPx_C3(image, rowPos);
        }
    }

    return outMat;
}
Ejemplo n.º 6
0
SEXP CombineSubMapsTransSimple(BigMatrix *oneVox_allSubs, SEXP allVoxs_allSubs, index_type seed, double *pVoxs, index_type nvoxs, index_type nsubs) {
    //using namespace Rcpp;
    
    BMAccessorType outMat( *oneVox_allSubs );
    
    if (nvoxs != oneVox_allSubs->ncol())
        ::Rf_error("nvoxs must equal oneVox_allSubs->ncol");
    if (nsubs != oneVox_allSubs->nrow())
        ::Rf_error("nsubs must equal oneVox_allSubs->nrow");
    
    // loop through each subject's map
    index_type s = 0;
    index_type v = 0;
    index_type vv = 0;
    LDOUBLE x = 0;
    LDOUBLE delta = 0;
    LDOUBLE mean = 0;
    LDOUBLE M2 = 0;
    LDOUBLE stdev = 0;
//    CType *inCol;
    CType *outCol;
    LDOUBLE scaled_x;
    BigMatrix *allVoxs_oneSub;
    SEXP Rp;
    SEXP tmp;
    //RObject RallVoxs_oneSub;
    for (s=0; s < nsubs; ++s) {
        PROTECT(tmp = VECTOR_ELT(allVoxs_allSubs, s));
        //RallVoxs_oneSub(tmp);
        //Rp = RallVoxs_oneSub.slot("address");
        PROTECT(Rp = GET_SLOT(tmp, install("address")));
        //tmp = allVoxs_allSubs[s];
        //RObject RallVoxs_oneSub(tmp);
        //Rp = RallVoxs_oneSub.slot("address");
        allVoxs_oneSub = reinterpret_cast<BigMatrix*>(R_ExternalPtrAddr(Rp));
        UNPROTECT(2);
        BMAccessorType inMat( *allVoxs_oneSub );
        
        for (v=0; v < nvoxs; ++v) {
            vv = static_cast<index_type>(pVoxs[v]-1);
            outMat[v][s] = static_cast<CType>(inMat[vv][seed]);
        }
    }
    
    return R_NilValue;
}
Ejemplo n.º 7
0
static boost::numeric::ublas::matrix<double> transformAsMatrix(const tf::Transform& bt) 
{
   boost::numeric::ublas::matrix<double> outMat(4,4);
 
   //  double * mat = outMat.Store();
 
   double mv[12];
   bt.getBasis().getOpenGLSubMatrix(mv);
 
   btVector3 origin = bt.getOrigin();
 
   outMat(0,0)= mv[0];
   outMat(0,1)  = mv[4];
   outMat(0,2)  = mv[8];
   outMat(1,0)  = mv[1];
   outMat(1,1)  = mv[5];
   outMat(1,2)  = mv[9];
   outMat(2,0)  = mv[2];
   outMat(2,1)  = mv[6];
   outMat(2,2) = mv[10];
 
   outMat(3,0)  = outMat(3,1) = outMat(3,2) = 0;
   outMat(0,3) = origin.x();
   outMat(1,3) = origin.y();
   outMat(2,3) = origin.z();
   outMat(3,3) = 1;

 
   return outMat;
};
int main(int argc, char ** argv){
  NcError error(NcError::verbose_nonfatal);

  try{
    std::string inFile;
    std::string outFile;
    std::string varName;
    std::string inList;
//    bool calcStdDev;

    BeginCommandLine()
      CommandLineString(inFile, "in", "");
      CommandLineString(inList, "inlist","");
      CommandLineString(varName, "var", "");
      CommandLineString(outFile, "out", "");
  //    CommandLineBool(calcStdDev, "std");
      ParseCommandLine(argc, argv);
    EndCommandLine(argv)
    AnnounceBanner();
  

    if ((inFile != "") && (inList != "")){
      _EXCEPTIONT("Can only open one file (--in) or list (--inlist).");
    }

    //file list vector
    std::vector<std::string> vecFiles;
    if (inFile != ""){
      vecFiles.push_back(inFile);
    }
    if (inList != ""){
      GetInputFileList(inList,vecFiles);
    }

    //open up first file
    NcFile readin(vecFiles[0].c_str());
    if (!readin.is_valid()){
      _EXCEPTION1("Unable to open file %s for reading",\
        vecFiles[0].c_str());
    }
    int tLen,latLen,lonLen;

    NcDim * time = readin.get_dim("time");
    tLen = time->size();
    NcVar * timeVar = readin.get_var("time");

    NcDim * lat = readin.get_dim("lat");
    latLen = lat->size();
    NcVar * latVar = readin.get_var("lat");

    NcDim * lon = readin.get_dim("lon");
    lonLen = lon->size();
    NcVar * lonVar = readin.get_var("lon");

    //read input variable
    NcVar * inVar = readin.get_var(varName.c_str());
    //Create output matrix
    DataMatrix<double> outMat(latLen,lonLen);
    densCalc(inVar,outMat);
    //Option for calculating the yearly standard deviation 
 /*   if (calcStdDev){
      for (int a=0; a<latLen; a++){
        for (int b=0; b<lonLen; b++){
          storeMat[0][a][b] = outMat[a][b];
        }
      }
    }
*/
    //If multiple files, add these values to the output
    if (vecFiles.size()>1){
      DataMatrix<double> addMat(latLen,lonLen);
      std::cout<<"There are "<<vecFiles.size()<<" files."<<std::endl;
      for (int v=1; v<vecFiles.size(); v++){
        NcFile addread(vecFiles[v].c_str());
        NcVar * inVar = addread.get_var(varName.c_str());
        densCalc(inVar,addMat);
        for (int a=0; a<latLen; a++){
          for (int b=0; b<lonLen; b++){
            outMat[a][b]+=addMat[a][b];
          }
        }
/*        if (calcStdDev){
          for (int a=0; a<latLen; a++){
            for (int b=0; b<lonLen; b++){
              storeMat[v][a][b] = addMat[a][b];
            }
          }
        }*/
        addread.close(); 
      }
      //Divide output by number of files
      double div = 1./((double) vecFiles.size());
      for (int a=0; a<latLen; a++){
        for (int b=0; b<lonLen; b++){
          outMat[a][b]*=div;
        }
      }
    }
 
    NcFile readout(outFile.c_str(),NcFile::Replace, NULL,0,NcFile::Offset64Bits);
    NcDim * outLat = readout.add_dim("lat", latLen);
    NcDim * outLon = readout.add_dim("lon", lonLen);
    NcVar * outLatVar = readout.add_var("lat",ncDouble,outLat);
    NcVar * outLonVar = readout.add_var("lon",ncDouble,outLon);
    std::cout<<"Copying dimension attributes."<<std::endl;
    copy_dim_var(latVar,outLatVar);
    copy_dim_var(lonVar,outLonVar);

    std::cout<<"Creating density variable."<<std::endl;

    NcVar * densVar = readout.add_var("dens",ncDouble,outLat,outLon);
    densVar->set_cur(0,0);
    densVar->put((&outMat[0][0]),latLen,lonLen);

/*    if (calcStdDev){
      NcVar * stdDevVar = readout.add_var("stddev", ncDouble,outLat,outLon);
      DataMatrix<double> stdDevMat(latLen,lonLen);
      yearlyStdDev(storeMat,vecFiles.size(),latLen,lonLen,stdDevMat);
      stdDevVar->set_cur(0,0);
      stdDevVar->put(&(stdDevMat[0][0]),latLen,lonLen);
      std::cout<<" created sd variable"<<std::endl;

    }
*/
    readout.close();
    readin.close();
  } 
  catch (Exception &e){
    std::cout<<e.ToString()<<std::endl;
  }
}
Ejemplo n.º 9
0
SEXP CombineSubMaps(BigMatrix *oneVox_allSubs, SEXP allVoxs_allSubs, index_type seed, double *pVoxs, index_type nvoxs, index_type nsubs) {
    //using namespace Rcpp;
    
    BMAccessorType outMat( *oneVox_allSubs );
    
    if (nsubs != oneVox_allSubs->ncol())
        Rf_error("nsubs must equal oneVox_allSubs->ncol");
    if (nvoxs != oneVox_allSubs->nrow())
        Rf_error("nsubs must equal oneVox_allSubs->nrow");
    
    // loop through each subject's map
    index_type s = 0;
    index_type v = 0;
    index_type vv = 0;
    LDOUBLE x = 0;
    LDOUBLE delta = 0;
    LDOUBLE mean = 0;
    LDOUBLE M2 = 0;
    LDOUBLE stdev = 0;
//    CType *inCol;
    CType *outCol;
    LDOUBLE scaled_x;
    BigMatrix *allVoxs_oneSub;
    SEXP Rp;
    SEXP tmp;
    //RObject RallVoxs_oneSub;
    for (s=0; s < nsubs; ++s) {
        PROTECT(tmp = VECTOR_ELT(allVoxs_allSubs, s));
        //RallVoxs_oneSub(tmp);
        //Rp = RallVoxs_oneSub.slot("address");
        PROTECT(Rp = GET_SLOT(tmp, install("address")));
        //tmp = allVoxs_allSubs[s];
        //RObject RallVoxs_oneSub(tmp);
        //Rp = RallVoxs_oneSub.slot("address");
        allVoxs_oneSub = reinterpret_cast<BigMatrix*>(R_ExternalPtrAddr(Rp));
        UNPROTECT(2);
        BMAccessorType inMat( *allVoxs_oneSub );
        
//        inCol = inMat[seed];
        delta = mean = M2 = stdev = 0;
        for (v=0; v < nvoxs; ++v) {
            // todo: add checking for NaN...but shouldn't really have any!
            // maybe can also pass the exact list of voxs to loop through!
            // if (!ISNAN(pColumn[curj]))
            // NA_REAL
            vv = static_cast<index_type>(pVoxs[v]-1);
            x = static_cast<LDOUBLE>(inMat[vv][seed]);
            delta = x - mean;
            mean = mean + delta/static_cast<LDOUBLE>(v+1);
            M2 = M2 + delta*(x - mean);
        }
        stdev = sqrt(M2/(static_cast<LDOUBLE>(nvoxs-1)));
        //printf("mean: %f; stdev: %f\n", mean, stdev);
        
        outCol = outMat[s];
        for (v=0; v < nvoxs; ++v) {
            vv = static_cast<index_type>(pVoxs[v]-1);
            scaled_x = (static_cast<LDOUBLE>(inMat[vv][seed])-mean)/stdev;
            outCol[v] = static_cast<CType>(scaled_x);
        }
    }
    
    return R_NilValue;
}
Ejemplo n.º 10
0
void expm_eigen(int n, double *rz, double *out)
{
    Eigen::Map< Eigen::MatrixXd > inMat(rz, n, n);
    Eigen::Map< Eigen::MatrixXd > outMat(out, n, n);
    outMat = inMat.exp();
}
Ejemplo n.º 11
0
SEXP invariantSimilarityHelper(
  typename itk::Image< float , ImageDimension >::Pointer image1,
  typename itk::Image< float , ImageDimension >::Pointer image2,
  SEXP r_thetas, SEXP r_thetas2, SEXP r_thetas3,
  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 thetas2( r_thetas2 );
  Rcpp::NumericVector thetas3( r_thetas3 );
  Rcpp::IntegerVector doReflection( r_doreflection );
  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 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> axis1;
    itk::Vector<RealType, ImageDimension> axis2;
    itk::Vector<RealType, ImageDimension> axis3;
    for( unsigned int d = 0; d < ImageDimension; d++ )
      {
      axis1[d] = evec_tert[d];
      axis2[d] = evec1_2ndary[d];
      axis3[d] = evec1_primary[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 < thetas.size(); i++ )
      {
      RealType ang1 = thetas[i];
      RealType ang2 = 0; // FIXME should be psi
      RealType ang3 = 0; // FIXME should be psi
      if( ImageDimension == 3 )
        {
        for ( unsigned int jj = 0; jj < thetas2.size(); jj++ )
        {
        ang2=thetas2[jj];
        for ( unsigned int kk = 0; kk < thetas3.size(); kk++ )
        {
        ang3=thetas3[kk];
        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->Rotate3D(axis3, ang3, 1);
//        affinesearch->Scale( bestscale ); // BUG: annoying, cant do for rigid
        simmer->SetMatrix(  affinesearch->GetMatrix() );
        parametersList.push_back( simmer->GetParameters() );
        } // kk
        }
        }
      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();
        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();
    unsigned int ncols = mstartOptimizer->GetBestParameters().Size() +
      1 + ImageDimension;
    Rcpp::NumericMatrix outMat( metricvalues.size(), ncols );
    for ( unsigned int k = 0; k < metricvalues.size(); k++ )
      {
      outMat( k, 0 ) = metricvalues[ k ];
      typename MetricType::ParametersType resultParams =
        mstartOptimizer->GetParametersList( )[ k ];
      for ( unsigned int kp = 0; kp < resultParams.Size(); kp++ )
        {
        outMat( k, kp + 1 ) = resultParams[ kp ];
        }
      // set the fixed parameters
      unsigned int baseind = resultParams.Size() + 1;
      for ( unsigned int kp = 0; kp < ImageDimension; kp++ )
        outMat( k, baseind + kp ) = bestaffine->GetFixedParameters()[ kp ];
      }
    return Rcpp::wrap( outMat );
    }
  else
    {
    Rcpp::NumericMatrix outMat( 1, 1 );
    outMat( 0, 0 ) = 0;
    return Rcpp::wrap( outMat );
    }
}