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());
}
void vnl_flann::search(const vnl_matrix<double> & query_data,
vcl_vector<vcl_vector<int> > & indices,
vcl_vector<vcl_vector<double> > & dists, int knn) const
{
const int dim = (int)query_data.cols();
assert(dim == dim_);
Matrix<double> query_data_wrap((double *)query_data.data_block(), (int)query_data.rows(), dim);
index_.knnSearch(query_data_wrap, indices, dists, knn, flann::SearchParams(128));
}
/*
Matlab code in cpd_G.m:
k=-2*beta^2;
[n, d]=size(x); [m, d]=size(y);
G=repmat(x,[1 1 m])-permute(repmat(y,[1 1 n]),[3 2 1]);
G=squeeze(sum(G.^2,2));
G=G/k;
G=exp(G);
*/
void ComputeGaussianKernel(const vnl_matrix<double>& model,
const vnl_matrix<double>& ctrl_pts,
vnl_matrix<double>& G, vnl_matrix<double>& K,
double lambda) {
int m,n,d;
m = model.rows();
n = ctrl_pts.rows();
d = ctrl_pts.cols();
//asssert(model.cols()==d);
//assert(lambda>0);
G.set_size(m,n);
GaussianAffinityMatrix(model.data_block(), ctrl_pts.data_block(),
m, n, d, lambda, G.data_block());
if (model == ctrl_pts) {
K = G;
} else {
K.set_size(n,n);
GaussianAffinityMatrix(ctrl_pts.data_block(), ctrl_pts.data_block(),
n, n, d, lambda, K.data_block());
}
}
vnl_matrix< std::complex <float> > myconvolve( vnl_matrix< std::complex<float> > &mat1, vnl_matrix< std::complex <float> > &mat2)
{
//printf("mat1.size() = %d %d\n",mat1.rows(),mat1.cols());
//printf("mat2.size() = %d %d\n",mat2.rows(),mat2.cols());
typedef itk::Image<unsigned short, 2> UShortImageType;
//writeImage<UShortImageType>(vnlmat_to_itkimage<unsigned short,float>(cpx_to_abs(mat1)),"testmat1.tif");
//writeImage<UShortImageType>(vnlmat_to_itkimage<unsigned short,float>(cpx_to_abs(mat2),255),"testmat2.tif");
int tsize = mat1.rows()*mat1.cols()*sizeof(fftwf_complex);
fftwf_complex *in = (fftwf_complex*)fftwf_malloc(tsize);
fftwf_complex *out = (fftwf_complex*)fftwf_malloc(tsize);
printf("Started planning..\n");
fftwf_plan plan;
#pragma omp critical
{
plan = fftwf_plan_dft_2d(mat1.rows(),mat1.cols(),in, out,FFTW_FORWARD,FFTW_MEASURE);
}
printf("Finished planning \n");
memcpy(in,mat1.data_block(),tsize);
fftwf_execute_dft(plan,in,out);
printf("executed fftwf\n");
//fftwf_destroy_plan(plan);
memcpy(mat1.data_block(),out,tsize);
//fftwf_free(in);
//fftwf_free(out);
//in = (fftwf_complex*)fftwf_malloc(tsize);
//out = (fftwf_complex*)fftwf_malloc(tsize);
//plan = fftwf_plan_dft_2d(mat1.rows(),mat1.cols(),in, out,FFTW_FORWARD,FFTW_MEASURE);
memcpy(in,mat2.data_block(),tsize);
fftwf_execute_dft(plan,in,out);
printf("executed fftwf\n");
fftwf_destroy_plan(plan);
memcpy(mat2.data_block(),out,tsize);
//fftwf_free(in);
//fftwf_free(out);
vnl_matrix< std::complex < float > > mat3(mat1.rows(),mat1.cols());
float factor = mat1.rows()*mat1.cols();
for(int xco = 0; xco < mat1.rows(); ++xco)
{
for(int yco = 0; yco < mat1.cols(); ++yco)
{
mat3[xco][yco] = mat1[xco][yco]/factor * mat2[xco][yco];
}
}
//in = (fftwf_complex*)fftwf_malloc(tsize);
//out = (fftwf_complex*)fftwf_malloc(tsize);
#pragma omp critical
{
plan = fftwf_plan_dft_2d(mat1.rows(),mat1.cols(),in, out,FFTW_BACKWARD,FFTW_MEASURE);
}
memcpy(in,mat3.data_block(),tsize);
fftwf_execute_dft(plan,in,out);
printf("executed fftwf\n");
fftwf_destroy_plan(plan);
memcpy(mat3.data_block(),out,tsize);
//writeImage<UShortImageType>(vnlmat_to_itkimage<unsigned short,float>(cpx_to_abs(mat3)),"testmat3.tif");
fftwf_free(in);
fftwf_free(out);
//std::cout<<mat3;
return mat3;
