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