// 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);
}
}
}
}
