// 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);
}
}
}
/// \brief Unscented transform of process Sigma points
void UnscentedKalmanFilter::utf(vnl_matrix<double> X, vnl_vector<double> u,
vnl_vector<double> &y, vnl_matrix<double> &Y, vnl_matrix<double> &P, vnl_matrix<double> &Y1)
{
// determine number of sigma points
unsigned int L = X.cols();
// zero output matrices
y.fill(0.0); Y.fill(0.0);
// transform the sigma points and put them as columns in a matrix Y
for( int k = 0; k < L; k++ )
{
vnl_vector<double> xk = X.get_column(k);
vnl_vector<double> yk(N);
f(xk,u,yk);
Y.set_column(k,yk);
// add each transformed point to the weighted mean
y = y + Wm.get(0,k)*yk;
}
// create a matrix with each column being the weighted mean
vnl_matrix<double> Ymean(N,L);
for( int k = 0; k < L; k++ )
Ymean.set_column(k,y);
// set the matrix of difference vectors
Y1 = Y-Ymean;
// calculate the covariance matrix output
vnl_matrix<double> WC(L,L,0.0);
WC.set_diagonal(Wc.get_row(0));
P = Y1*WC*Y1.transpose();
}
void TpsRegistration::ComputeGradient(const double lambda,
const vnl_matrix<double>& gradient, vnl_matrix<double>& grad_all) {
grad_all.fill(0);
if (lambda > 0) {
grad_all.update(2 * lambda * kernel_ * param_tps_, d_ + 1);
}
grad_all += basis_.transpose() * gradient;
}
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;
}
}
}
