/*!
\fn generateClusterMembershipForFeatures
Params:
@param1: cv::flann::Index flann_index
FLANN Index trained before
@param2: cv::Mat featureVector
new feature vector needed to find nearest neighbor points
@param3: int k (default value is 1)
number of nearest neighbor of each new feature vector
return
cv::Mat indices
nearest neighbor index
description:
Find the nearest neighbor index of each new feature vector
*/
cv::Mat WordModel::generateClusterMembershipForFeatures(cv::flann::Index flann_index, cv::Mat featureVector, int k/* =1 */)
{
cv::Mat indices = cv::Mat::zeros(featureVector.rows, k, CV_32S);
cv::Mat distances = cv::Mat::zeros(featureVector.rows, k, CV_32F);
int numbreOfCheckers = 100;
flann_index.knnSearch(featureVector, indices, distances, k);
return indices;
}
double IterativeTranslationFitter::getModelFitScore(const std::vector<cv::Vec3f>& cloud, const cv::Point3f& position,
boost::function<double(double)> kernel,
cv::flann::Index &search) const
{
double inlier_count = 0;
std::vector<int> indices(1);
std::vector<float> distances(1);
cv::Mat_<float> points(1, 3);
for (std::vector<cv::Point3f>::const_iterator mIt = model_points_.begin(); mIt != model_points_.end(); ++mIt) {
points(0, 0) = mIt->x + position.x;
points(0, 1) = mIt->y + position.y;
points(0, 2) = mIt->z + position.z;
search.knnSearch(points, indices, distances, 1);
inlier_count += kernel(sqrt(distances[0]));
}
return inlier_count / model_points_.size();
}
double Util::GetSearchRadius(cv::flann::Index &myKdTree, const cv::Mat &features, int nMaxSearch, float percent = 0.02f)
{
//3.5 计算radius,目标2%
double maxDistanceSum = 0.0;
int numpts = features.rows;
//int DIMENSION = features.cols;
int knn = numpts*percent;
if(knn > nMaxSearch)
knn = nMaxSearch;
int nchecks = 2*knn > nMaxSearch ? 2*knn : nMaxSearch;
for(int i = 0; i < numpts; i++){
cv::Mat indices;
cv::Mat dists;
myKdTree.knnSearch(features.row(i), indices, dists, knn, cv::flann::SearchParams(nchecks));
//std::cout << distances << std::endl;
float localMax = dists.at<float>(0, knn-1);
//std::cout << localMax << std::endl;
maxDistanceSum += localMax;
}
double search_radius = maxDistanceSum / numpts; //average
return search_radius;
}
inline void nearestKSearch(cv::flann::Index &index, const Model &model, int k, std::vector<int> &indices, std::vector<float> &distances)
{
indices.resize(k);
distances.resize(k);
index.knnSearch(model.second, indices, distances, k, cv::flann::SearchParams(512));
}