// todo: add one more version when the model is same as ctrl_pts // reference: Landmark-based Image Analysis, Karl Rohr, p195 void ComputeTPSKernel(const vnl_matrix<double>& model, const vnl_matrix<double>& ctrl_pts, vnl_matrix<double>& U, vnl_matrix<double>& K) { int m = model.rows(); int n = ctrl_pts.rows(); int d = ctrl_pts.cols(); //asssert(model.cols()==d==(2|3)); K.set_size(n, n); K.fill(0); U.set_size(m, n); U.fill(0); double eps = 1e-006; vnl_vector<double> v_ij; for (int i = 0; i < m; ++i) { for (int j = 0; j < n; ++j) { v_ij = model.get_row(i) - ctrl_pts.get_row(j); if (d == 2) { double r = v_ij.squared_magnitude(); if (r > eps) { U(i, j) = r * log(r) / 2; } } else if (d == 3) { double r = v_ij.two_norm(); U(i, j) = -r; } } } for (int i = 0; i < n; ++i) { for (int j = i + 1; j < n; ++j) { v_ij = ctrl_pts.get_row(i) - ctrl_pts.get_row(j); if (d == 2) { double r = v_ij.squared_magnitude(); if (r > eps) { K(i, j) = r * log(r) / 2; } } else if (d == 3) { double r = v_ij.two_norm(); K(i, j) = -r; } } } for (int i = 0; i < n; ++i) { for (int j = 0; j < i; ++j) { K(i, j) = K(j, i); } } }
void pick_indices(const vnl_matrix<double>&dist, vnl_matrix<int>&pairs, 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); std::vector<int> row_index,col_index; 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; } } } } pairs.set_size(2, row_index.size()); for (int i = 0; i<pairs.cols(); ++i){ pairs(0,i) = row_index[i]; pairs(1,i) = col_index[i]; } }
/// @param v1 a vector to rotate /// @param v2 a vector to which v1 rotates /// @param rotation an output matrix that stores a transform v1 to v2 void vtkIO::rotateVector(const double *x1, const double *x2, vnl_matrix<double> &rotation) { double v1[3], v2[3], vn[3]; /// Compute the norm for v1 and v2 const double n1 = vtkMath::Norm(x1); const double n2 = vtkMath::Norm(x2); for (int i = 0; i < 3; i++) { v1[i] = x1[i] / n1; v2[i] = x2[i] / n2; } /// Compute the cross product vtkMath::Cross(v1, v2, vn); double nn = vtkMath::Normalize(vn); double theta = asin(nn); /// sin(theta) double st = nn; /// 1 - cos(theta) double cct = 1 - cos(theta); rotation.set_size(3, 3); rotation[0][0] = 1 + cct * (vn[0]*vn[0] - 1); rotation[0][1] = 0 - vn[2] * st + cct * (vn[0]*vn[1]); rotation[0][2] = 0 + vn[1] * st + cct * (vn[0]*vn[2]); rotation[1][0] = 0 + vn[2] * st + cct * (vn[0]*vn[1]); rotation[1][1] = 1 + cct * (vn[1]*vn[1] - 1); rotation[1][2] = 0 - vn[0] * st + cct * (vn[1] * vn[2]); rotation[2][0] = 0 - vn[1] * st + cct * (vn[0] * vn[2]); rotation[2][1] = 0 + vn[0] * st + cct * (vn[1] * vn[2]); rotation[2][2] = 1 + cct * (vn[2]*vn[2] - 1); }
int select_points(const vnl_matrix<double>&pts, const std::vector<int>&index, vnl_matrix<double>& selected) { int n = index.size(); int d = pts.cols(); selected.set_size(n,d); for (int i = 0; i < n; ++i) { selected.update(pts.extract(1, d, index[i]), i); } return n; }
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); } }
void f(const vnl_matrix<double>& model, const vnl_matrix<double>& scene, double threshold, vnl_matrix<double>& extracted_model, vnl_matrix<double>& extracted_scene) { vnl_matrix<double> dist; vnl_matrix<int> pairs; ComputeSquaredDistanceMatrix(model, scene, dist); pick_indices(dist, pairs, threshold*threshold); std::cout << "distance threshold : " << threshold << std::endl; int j, n = pairs.cols(); int d = model.cols(); extracted_model.set_size(n,d); extracted_scene.set_size(n,d); std::cout << "# of matched point pairs : " << n << std::endl; for (j=0; j<n; ++j) { extracted_model.set_row(j,model.get_row(pairs(0,j))); } for (j=0; j<n; ++j) { extracted_scene.set_row(j,scene.get_row(pairs(1,j))); } }
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 ComputeSquaredDistanceMatrix(const vnl_matrix<double>& A, const vnl_matrix<double>& B, vnl_matrix<double>& D) { int m = A.rows(); int n = B.rows(); //asssert(A.cols()==B.cols()); D.set_size(m,n); vnl_vector<double> v_ij; for (int i = 0; i < m; ++i) { for (int j = 0; j < n; ++j) { v_ij = A.get_row(i) - B.get_row(j); D(i,j) = v_ij.squared_magnitude(); } } }
/* 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()); } }
void ExtractMatchingPairs( const vnl_matrix<T>& model, const vnl_matrix<T>& scene, const T& threshold, vnl_matrix<T>& extracted_model, vnl_matrix<T>& extracted_scene) { vnl_matrix<T> dist; vnl_matrix<int> pairs; ComputeSquaredDistanceMatrix<T>(model, scene, dist); PickIndices<T>(dist, pairs, threshold*threshold); std::cout << "distance threshold : " << threshold << std::endl; int n = pairs.cols(); int d = model.cols(); extracted_model.set_size(n, d); extracted_scene.set_size(n, d); std::cout << "# of matched point pairs : " << n << std::endl; for (int j = 0; j < n; ++j) { extracted_model.set_row(j,model.get_row(pairs(0, j))); } for (int j = 0; j < n; ++j) { extracted_scene.set_row(j,scene.get_row(pairs(1, j))); } }
// ------------------------------------------------------------------------ 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); } } }
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 } } }
void ComputeTPSKernelU(const vnl_matrix<double>& model, const vnl_matrix<double>& ctrl_pts, vnl_matrix<double>& U) { int m = model.rows(); int n = ctrl_pts.rows(); int d = ctrl_pts.cols(); //asssert(model.cols()==d==(2|3)); //K.set_size(n, n); //K.fill(0); U.set_size(m, n); U.fill(0); double eps = 1e-006; vnl_vector<double> v_ij; for (int i = 0; i < m; ++i) { for (int j = 0; j < n; ++j) { v_ij = model.get_row(i) - ctrl_pts.get_row(j); double r = v_ij.two_norm(); U(i, j) = -r; } } }
// 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); } } } }