void denormalize(vnl_matrix<double>& x, const vnl_vector<double>& centroid, const double scale) {
int n = x.rows();
if (n==0) return;
int d = x.cols();
for (int i = 0; i < n; ++i) {
x.set_row(i, x.get_row(i) * scale + centroid);
}
}
void normalize_same(vnl_matrix<double>& x,
vnl_vector<double>& centroid, double& scale) {
int n = x.rows();
if (n==0) return;
int d = x.cols();
for (int i = 0; i < n; ++i) {
x.set_row(i, (x.get_row(i) - centroid) / scale);
}
}
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)));
}
}
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 normalize(vnl_matrix<double>& x,
vnl_vector<double>& centroid, double& scale) {
int n = x.rows();
if (n==0) return;
int d = x.cols();
centroid.set_size(d);
vnl_vector<double> col;
for (int i = 0; i < d; ++i) {
col = x.get_column(i);
centroid(i) = col.mean();
}
for (int i = 0; i < n; ++i) {
x.set_row(i, x.get_row(i) - centroid);
}
scale = x.frobenius_norm() / sqrt(double(n));
x = x / scale;
}
