/*!
\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;
}
TyErrorId initialize(AnnotatorContext &ctx)
{
outInfo("initialize");
resourcesPath = ros::package::getPath("rs_resources") + '/';
ctx.extractValue("DeCafH5File", h5_file);
ctx.extractValue("DeCafListFile", list_file);
ctx.extractValue("DeCafKDTreeIndices", kdtree_file);
ctx.extractValue("DeCafKNeighbors", k);
ctx.extractValue("caffe_model_file", caffe_model_file);
ctx.extractValue("caffe_trained_file", caffe_trained_file);
ctx.extractValue("caffe_mean_file", caffe_mean_file);
ctx.extractValue("caffe_label_file", caffe_label_file);
ctx.extractValue("asGT", asGT);
outInfo(h5_file);
outInfo(list_file);
outInfo(kdtree_file);
outInfo(caffe_model_file);
outInfo(caffe_trained_file);
outInfo(caffe_mean_file);
outInfo(caffe_label_file);
// Check if the data has already been saved to disk
if(!boost::filesystem::exists(resourcesPath + h5_file) ||
!boost::filesystem::exists(resourcesPath + list_file) ||
!boost::filesystem::exists(resourcesPath + kdtree_file) ||
!boost::filesystem::exists(resourcesPath + caffe_model_file) ||
!boost::filesystem::exists(resourcesPath + caffe_trained_file) ||
!boost::filesystem::exists(resourcesPath + caffe_mean_file) ||
!boost::filesystem::exists(resourcesPath + caffe_label_file))
{
outError("files not found!");
return UIMA_ERR_USER_ANNOTATOR_COULD_NOT_INIT;
}
caffeProxyObj = std::make_shared<CaffeProxy>(resourcesPath + caffe_model_file,
resourcesPath + caffe_trained_file,
resourcesPath + caffe_mean_file,
resourcesPath + caffe_label_file);
flann::Matrix<float> data;
loadFileList(models, resourcesPath + list_file);
flann::load_from_file(data, resourcesPath + h5_file, "training_data");
outInfo("Training data found. Loaded " << data.rows << " models from " << h5_file << "/" << list_file);
this->data = cv::Mat(data.rows, data.cols, CV_32F, data.ptr()).clone();
index.build(this->data, cv::flann::KDTreeIndexParams());
return UIMA_ERR_NONE;
}
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));
}
