std::vector<vnl_vector<double> > EigenVectors(const vnl_matrix<double> &M)
	{
		vnl_symmetric_eigensystem<double> Eigs(M);
		
		std::vector<vnl_vector<double> > EVecs;
		for(unsigned int i = 0; i < M.columns(); i++)
			EVecs.push_back(Eigs.get_eigenvector(i));
		
			return EVecs;
		
	}

void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::CreateDirMatrix(const std::vector< DirectionType >& dir, vnl_matrix<double>& sphCoords)
{
  sphCoords.set_size(3, dir.size());
  for (unsigned int i=0; i<dir.size(); i++)
  {
    sphCoords(0, i) = dir[i](0);
    sphCoords(1, i) = dir[i](1);
    sphCoords(2, i) = dir[i](2);
  }
}

vnl_vector<double> Rigid2DTransform::GetParametersFromMatrix(const vnl_matrix<double> &transformMatrix) const {
	vnl_vector<double> params(m_nbParams);

	vnl_matrix<double> rotmat = transformMatrix.extract(m_nbDimensions,m_nbDimensions,0,0);

	params(0) = transformMatrix(0,m_nbDimensions);
	params(1) = transformMatrix(1,m_nbDimensions);

	params(2) = psciob::GetAngleFrom2DRotationMatrix(rotmat);

	return params;
}

vnl_matrix <double> MCLR_SM::Normalize_Feature_Matrix_1(vnl_matrix<double> feats, vnl_vector<double> vector_1, vnl_vector<double> vector_2)
{
	std_vec = vector_1;
	mean_vec = vector_2;
	

//The last column is the training column 
	for(int i = 0; i<feats.columns() ; ++i)
	{
		vnl_vector<double> temp_col = feats.get_column(i);
		if(std_vec(i) > 0)
		{	
			for(int j =0; j<temp_col.size() ; ++j)
				temp_col[j] = (temp_col[j] - mean_vec(i))/std_vec(i) ;
		}
	
		feats.set_column(i,temp_col);
	}

	return feats;
}

//f  = f./repmat(sum(f,1),[classN,1]);
vnl_matrix<double> MCLR_SM::Normalize_F_Sum(vnl_matrix<double> f)
{
	// Matrix for normalization
	vnl_matrix<double> norm_matrix(f.rows(),f.cols());
	vnl_vector<double> norm_matrix_row;
	norm_matrix_row.set_size(f.cols());
	
	 //repmat(sum(f,1),[classN,1]);
	for(int i=0;i<f.cols();++i)
	{
      double sum = 0 ;  
	  for(int j=0;j<no_of_classes;++j)
		{
			sum = sum + f(j,i);
		}
		norm_matrix_row(i) = sum;
	}


    for(int i=0;i<no_of_classes;++i)
	{
		norm_matrix.set_row(i,norm_matrix_row); 
	}

// f  = f./repmat(sum(f,1),[classN,1]);
	for(int i=0;i<f.rows();++i)
	{
	  for(int j=0;j<f.cols();++j)
	   {
		  f(i,j) = f(i,j)/norm_matrix(i,j);
	   }
	}

	return f;
}

void mitk::GeneralizedLinearModel::EstimatePermutation(const vnl_matrix<double> &xData)
{
  v3p_netlib_integer rows = xData.rows();
  v3p_netlib_integer cols = xData.cols();

  if (m_AddConstantColumn)
    ++cols;

  v3p_netlib_doublereal *x = new v3p_netlib_doublereal[rows* cols];
  _UpdateXMatrix(xData, m_AddConstantColumn, x);
  v3p_netlib_doublereal *qraux = new v3p_netlib_doublereal[cols];
  v3p_netlib_integer *jpvt = new v3p_netlib_integer[cols];
  std::fill_n(jpvt,cols,0);
  v3p_netlib_doublereal *work = new v3p_netlib_doublereal[cols];
  std::fill_n(work,cols,0);
  v3p_netlib_integer job = 16;

  // Make a call to Lapack-DQRDC which does QR with permutation
  // Permutation is saved in JPVT.
  v3p_netlib_dqrdc_(x, &rows, &rows, &cols, qraux, jpvt, work, &job);

  double limit = std::abs(x[0]) * std::max(cols, rows) * std::numeric_limits<double>::epsilon();
  // Calculate the rank of the matrix
  int m_Rank = 0;
  for (int i = 0; i <cols; ++i)
  {
    m_Rank += (std::abs(x[i*rows + i]) > limit) ? 1 : 0;
  }
  // Create a permutation vector
  m_Permutation.set_size(m_Rank);
  for (int i = 0; i < m_Rank; ++i)
  {
    m_Permutation(i) = jpvt[i]-1;
  }

  delete x;
  delete qraux;
  delete jpvt;
  delete work;
}

int vtkPolyData2vnl_matrix(vtkPolyData* A, vnl_matrix<double>& matrix) 
{
	int dim = 3;
	int n = A->GetNumberOfPoints();
	matrix.set_size(n, dim);
	for(int i = 0;i < n; i++)
	{
		double *P = A->GetPoint(i);
		for(int j = 0;j < dim; j++ )
		matrix(i,j) = P[j];
	}
	return 1;
}

void pick_indices(const vnl_matrix<double>&dist,
    std::vector<int>&row_index, std::vector<int>&col_index,
    const double threshold) {
  int m = dist.rows();
  int n = dist.cols();
  vnl_vector<int> row_flag, col_flag;
  col_flag.set_size(n);  col_flag.fill(0);
  row_flag.set_size(n);  row_flag.fill(0);
  for (int i = 0; i < m; ++i) {
    double min_dist = dist.get_row(i).min_value();
    if (min_dist < threshold) {
      for (int j = 0; j < n; ++j){
        if (dist(i,j) == min_dist && col_flag[j] == 0){
          row_index.push_back(i);
          row_flag[i] = 1;
          col_index.push_back(j);
          col_flag[j] = 1;
        }
      }
    }
  }
}

double GaussTransform(const vnl_matrix<double>& A,
    const vnl_matrix<double>& B, double scale,
    vnl_matrix<double>& gradient) {
  // assert A.cols() == B.cols()
  return GaussTransform(A.data_block(), B.data_block(),
      A.rows(), B.rows(), A.cols(), scale,
      gradient.data_block());
}

// Fills the value of the xData-matrix into the x-matrix. Adds a constant
// column if required. Permutes the rows corresponding to the permutation vector.
static void _UpdatePermXMatrix(const vnl_matrix<double> &xData, bool addConstant, const vnl_vector<unsigned int> &permutation, vnl_matrix<double> &x)
{
  int rows = xData.rows();
  int cols = permutation.size();
  x.set_size(rows, cols);
  for (int r=0; r < rows; ++r)
  {
    for (int c=0; c<cols; ++c)
    {
      unsigned int newCol = permutation(c);
      if (!addConstant)
      {
        x(r, c) = xData(r,newCol);
      } else if (newCol == 0)
      {
        x(r, c) = 1.0;
      } else
      {
        x(r, c) = xData(r, newCol-1);
      }
    }
  }
}

//Reshape the matrix : columns first ; Similar to MATLAB
vnl_matrix<double> MCLR_SM::Reshape_Matrix(vnl_matrix<double>mat,int r,int c )
{
	if(mat.rows()*mat.cols() != r*c)
	{
		cout<< "Number of elements in the matrix/vector should be equal to the total number of elements in the reshaped matrix";
		getchar();
		exit(1);
	}
	
	vnl_matrix<double>reshaped_matrix;
	reshaped_matrix.set_size(r,c);
	int count = 0;
	
	for(int j=0;j<c;++j)	
	{
		for(int i=0;i<r;++i)
		{
			reshaped_matrix(i,j) = mat(count%mat.rows(),floor(static_cast<double>(count/mat.rows())));
			count++;
		}
	}
	return reshaped_matrix;
}

vnl_matrix<double> MCLR_SM::Get_F_Matrix(vnl_matrix<double> data_bias,vnl_matrix<double> w_temp)
{
	vnl_matrix<double> epow_matrix = w_temp.transpose()*data_bias;
	vnl_matrix<double> temp_f;
	temp_f.set_size(epow_matrix.rows(),epow_matrix.cols());
	for(int i=0;i<epow_matrix.rows();++i)
	{
	  for(int j=0;j<epow_matrix.cols();++j)
	   {
		  temp_f(i,j) = exp(epow_matrix(i,j));
	  }
	}
	return temp_f;
}

	bool CloseEnough(const vnl_matrix<double> &M1, const vnl_matrix<double> &M2, const double eps)
	{
		unsigned int NumRows = M1.rows();
		unsigned int NumCols = M1.columns();
		if((M2.rows() != NumRows) || (M2.columns() != NumCols))
		{
			std::cout << "Dimensions do not match!" << std::endl;
			return false;	
		}
		
		for(unsigned int r = 0; r < NumRows; r++)
		{
			for(unsigned int c = 0; c < NumCols; c++)
			{
				if(fabs(M1(r,c) - M2(r,c)) > eps)
				{
					std::cout << "Failed comparison: " << "M1: " << M1(r,c) << " M2: " << M2(r,c) << " diff: " << fabs(M1(r,c) - M2(r,c)) << std::endl;
					return false;
				}
			}
		}
		return true;	
	}

	vnl_double_3x3 Get3x3SubMatrix(const vnl_matrix<double> &M)
	{
		vnl_double_3x3 R;
		
		for(unsigned r = 0; r < 3; r++)
		{
			for(unsigned c = 0; c < 3; c++)
			{
				R.put(r,c, M.get(r,c));	
			}
		}
		
		return R;
	}

// Copy a vnl-matrix to an c-array with row-wise representation.
// Adds a constant column if required.
static void _UpdateXMatrix(const vnl_matrix<double> &xData, bool addConstant, v3p_netlib_doublereal *x)
{
  v3p_netlib_integer rows = xData.rows();
  v3p_netlib_integer cols = xData.cols();
  if (addConstant)
    ++cols;

  for (int r=0; r < rows; ++r)
  {
    for (int c=0; c <cols; ++c)
    {
      if (!addConstant)
      {
        x[c*rows + r] = xData(r,c);
      } else if (c == 0)
      {
        x[c*rows + r] = 1.0;
      } else
      {
        x[c*rows + r] = xData(r, c-1);
      }
    }
  }
}

// ------------------------------------------------------------------------
void computeTransform(const ImageType::Pointer &image, vnl_matrix<double> &transform)
{
	vnl_matrix<double> dirMat = image->GetDirection().GetVnlMatrix();
	transform.set_size(4,4);

	for(unsigned int i = 0; i < 3; i++)
	{
		for(unsigned int j = 0; j < 3; j++)
		{
			transform(i,j) = dirMat(i,j);
		}
	}


}

vnl_vector<double> Rigid3DTransform::GetParametersFromMatrix(const vnl_matrix<double> &transformMatrix) const {
	vnl_vector<double> params(m_nbParams);

	params(0) = transformMatrix(0,m_nbDimensions);
	params(1) = transformMatrix(1,m_nbDimensions);
	params(2) = transformMatrix(2,m_nbDimensions);

	vnl_vector<double> tmpvect = GetEulerAnglesFrom3DRotationMatrix(transformMatrix.extract(3,3,0,0));

	params(3) = tmpvect(0);
	params(4) = tmpvect(1);
	params(5) = tmpvect(2);

	return params;
}

vnl_matrix<double> FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::CalcShBasis(vnl_matrix<double>& sphCoords)
{
    int M = sphCoords.rows();
    int j, m; double mag, plm;
    vnl_matrix<double> shBasis;
    shBasis.set_size(M, m_NumCoeffs);

    for (int p=0; p<M; p++)
    {
        j=0;
        for (int l=0; l<=ShOrder; l=l+2)
            for (m=-l; m<=l; m++)
            {
                switch (m_Toolkit)
                {
                case FSL:
                plm = legendre_p<double>(l,abs(m),cos(sphCoords(p,0)));
                mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;

                if (m<0)
                    shBasis(p,j) = sqrt(2.0)*mag*cos(fabs((double)m)*sphCoords(p,1));
                else if (m==0)
                    shBasis(p,j) = mag;
                else
                    shBasis(p,j) = pow(-1.0, m)*sqrt(2.0)*mag*sin(m*sphCoords(p,1));
                    break;
                case MRTRIX:

                plm = legendre_p<double>(l,abs(m),-cos(sphCoords(p,0)));
                mag = sqrt((double)(2*l+1)/(4.0*M_PI)*factorial<double>(l-abs(m))/factorial<double>(l+abs(m)))*plm;
                if (m>0)
                    shBasis(p,j) = mag*cos(m*sphCoords(p,1));
                else if (m==0)
                    shBasis(p,j) = mag;
                else
                    shBasis(p,j) = mag*sin(-m*sphCoords(p,1));
                    break;
                }

                j++;
            }
    }
    return shBasis;
}

void FiniteDiffOdfMaximaExtractionFilter< PixelType, ShOrder, NrOdfDirections>
::Cart2Sph(const std::vector< DirectionType >& dir, vnl_matrix<double>& sphCoords)
{
    sphCoords.set_size(dir.size(), 2);

    for (unsigned int i=0; i<dir.size(); i++)
    {
        double mag = dir[i].magnitude();

        if( mag<0.0001 )
        {
            sphCoords(i,0) = M_PI/2; // theta
            sphCoords(i,1) = M_PI/2; // phi
        }
        else
        {
            sphCoords(i,0) = acos(dir[i](2)/mag); // theta
            sphCoords(i,1) = atan2(dir[i](1), dir[i](0)); // phi
        }
    }
}

vnl_vector<double> mitk::GeneralizedLinearModel::ExpMu(const vnl_matrix<double> &x)
{
  LogItLinking link;
  vnl_vector<double> mu(x.rows());
  int cols = m_B.size();
  for (unsigned int r = 0 ; r < mu.size(); ++r)
  {
    mu(r) = 0;
    for (int c = 0; c < cols; ++c)
    {
      if (!m_AddConstantColumn)
        mu(r) += x(r,c)*m_B(c);
      else if ( c == 0)
        mu(r) += m_B(c);
      else
        mu(r) += x(r,c-1)*m_B(c);
    }
    mu(r) = exp(-mu(r));
  }
  return mu;
}