void
nearestKSearch (float vfhSignature[])
{
std::vector<float> vec;
vec.resize(308);
for (int i =0; i < 308; i++){
vec[i] = vfhSignature[i];
}
// Query point
flann::Matrix<float> p = flann::Matrix<float>(new float[vec.size()], 1, vec.size());
memcpy (&p.data[0], &vec[0], p.cols * p.rows * sizeof (float));
k_indices = flann::Matrix<int>(new int[noOfNeighbors], 1, noOfNeighbors);
k_distances = flann::Matrix<float>(new float[noOfNeighbors], 1, noOfNeighbors);
ROS_INFO("%d",index->size());
index->knnSearch (p, k_indices, k_distances, noOfNeighbors, flann::SearchParams (512));
p.free();
}
void FeatureMatchThread::run()
{
Mat resultImg;
Mat grayImg;
cvtColor(cropImg,grayImg,CV_BGR2GRAY);
featureDetector.detect(grayImg,keyPoints);
featureExtractor.compute(grayImg,keyPoints,descriptors);
flannIndex.build(descriptors,flann::LshIndexParams(12,20,2),cvflann::FLANN_DIST_HAMMING);
while(true)
{
Mat captureImage_gray;
vector<KeyPoint> captureKeyPoints;
Mat captureDescription;
vector<DMatch> goodMatches;
cap >> captureImage;
if(captureImage.empty())
continue;
cvtColor(captureImage,captureImage_gray,CV_BGR2GRAY);
featureDetector.detect(captureImage_gray,captureKeyPoints);
featureExtractor.compute(captureImage_gray,captureKeyPoints,captureDescription);
Mat matchIndex(captureDescription.rows,2,CV_32SC1);
Mat matchDistance(captureDescription.rows,2,CV_32FC1);
flannIndex.knnSearch(captureDescription,matchIndex,matchDistance,2,flann::SearchParams());
for(int i=0;i<matchDistance.rows;i++)
{
if(matchDistance.at<float>(i,0) < 0.6 * matchDistance.at<float>(i,1))
{
DMatch dmatches(i,matchIndex.at<int>(i,0),matchDistance.at<float>(i,0));
goodMatches.push_back(dmatches);
}
}
drawMatches(captureImage,captureKeyPoints,cropImg,keyPoints,goodMatches,resultImg);
emit NewFeatureMatch(&resultImg);
imshow(WindowName,captureImage);
cv::setMouseCallback(WindowName,mouse_callback);
captureKeyPoints.clear();
goodMatches.clear();
waitKey(30);
}
return;
}
map<int,float> code(flann::Index& index, vector<vector<float> >& features, int knn = 5)
{
vector< vector<float> > codes;
for(int i = 0; i < features.size(); i++)
{
vector<float> dists;
vector<int> indices;
vector<float> data = features[i];
index.knnSearch(data, indices, dists, knn);
vector<float> thisCodes;
for(int j =0; j < knn; j++)
{
thisCodes.push_back( indices[j] );
thisCodes.push_back( dists[j] );
}
codes.push_back(thisCodes);
}
return pool(codes);
}
void SupervisedFilter::classify(Volume &volume,
flann::Index<flann::L2<short> > &index,
size_t nn) {
// Need same number of points in dataset and classification
// and same number of features in dataset and query.
if (this->dataset.rows != this->classifications.rows || this->dataset.cols != this->query.cols) {
std::cerr << "Error: Data dimensions mismatch" << std::endl;
return;
}
flann::Matrix<int> indices(new int[query.rows*nn], query.rows, nn);
flann::Matrix<float> distances(new float[query.rows*nn], query.rows, nn);
std::cout << "Beginning nn search\n";
index.knnSearch(query, indices, distances, nn, flann::SearchParams(128));
std::cout << "Making probability image\n";
size_t row = 0;
for (size_t x = 0; x < volume.width; ++x) {
for (size_t y = 0; y < volume.height; ++y) {
for (size_t z = 0; z < volume.depth; ++z) {
int feature_neighbours = 0;
for (size_t j = 0; j < nn; ++j) {
feature_neighbours += this->classifications[indices[row][j]][0];
}
volume(x,y,z) = feature_neighbours; // What is a good meassure here?
++row;
}
}
}
delete[] indices.ptr();
delete[] distances.ptr();
}
void runTest(const flann::Index<L2_Simple<float> >& index, SearchParams params)
{
start_timer("Searching Radius (single core)...");
params.cores = 1;
int single_neighbor_count = index.radiusSearch(query_, indices_single_, dists_single_, radius_, params);
printf("done (%g seconds)\n", stop_timer());
start_timer("Searching Radius (multi core)...");
params.cores = 0;
int multi_neighbor_count = index.radiusSearch(query_, indices_multi_, dists_multi_, radius_, params);
printf("done (%g seconds)\n", stop_timer());
EXPECT_EQ(single_neighbor_count, multi_neighbor_count);
printf("Checking results...\n");
float precision = compute_precision(indices_single_, indices_multi_);
EXPECT_GE(precision, 0.99);
printf("Precision: %g\n", precision);
}
void mouse_callback(int event,int x,int y,int flags, void* param)
{
switch(event)
{
case EVENT_MOUSEMOVE:
{
if(drawing_box)
{
box.width = x-box.x;
box.height = y-box.y;
Mat tmp = captureImage.clone();
cv::rectangle(tmp,Point(box.x,box.y),Point(box.x + box.width , box.y + box.height) , Scalar(0,255,0) , 3);
imshow(WindowName,tmp);
}
}
break;
case EVENT_LBUTTONDOWN:
{
drawing_box = true;
box = Rect(x,y,0,0);
}
break;
case EVENT_LBUTTONUP:
{
drawing_box = false;
if(box.width < 0)
{
box.x += box.width;
box.width *= -1;
}
if(box.height < 0)
{
box.y += box.height;
box.height *= -1;
}
//Â^¨ú¹Ï¹³
Mat copyCapture = captureImage.clone();
Mat tmp(copyCapture,box);
Mat grayImg;
cropImg = tmp;
cvtColor(cropImg,grayImg,CV_BGR2GRAY);
featureDetector.detect(grayImg,keyPoints);
featureExtractor.compute(grayImg,keyPoints,descriptors);
flannIndex.build(descriptors,flann::LshIndexParams(12,20,2),cvflann::FLANN_DIST_HAMMING);
}
break;
}
return;
}
void VFH::nearestKSearch (flann::Index<flann::ChiSquareDistance<float> > &index, const vfh_model &model,
int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances)
{
// Query point
flann::Matrix<float> p = flann::Matrix<float>(new float[model.second.size ()], 1, model.second.size ());
memcpy (&p.ptr ()[0], &model.second[0], p.cols * p.rows * sizeof (float));
indices = flann::Matrix<int>(new int[k], 1, k);
distances = flann::Matrix<float>(new float[k], 1, k);
index.knnSearch (p, indices, distances, k, flann::SearchParams (512));
delete[] p.ptr ();
}
//! Find the nearest neighbors in the descriptor space using FLANN.
void append_nearest_neighbors(
size_t i1,
const Set<OERegion, RealDescriptor>& keys1,
const Set<OERegion, RealDescriptor>& keys2,
vector<Match>& matches,
const flann::Matrix<float>& /*data2*/,
flann::Index<flann::L2<float>>& tree2,
float squared_ratio_thres,
Match::Direction dir,
bool self_matching, const KeyProximity& is_redundant, // self-matching
vector<int>& vec_indices,
vector<float>& vec_dists,
size_t num_max_neighbors) // internal storage parameters
{
// Prepare the query matrix
flann::Matrix<float> query{
const_cast<float *>(&(keys1.descriptors.matrix()(0, i1))),
1, keys1.descriptors.dimension()
};
// Prepare the indices and distances.
flann::Matrix<int> indices{ &vec_indices[0], 1, num_max_neighbors };
flann::Matrix<float> dists{ &vec_dists[0], 1, num_max_neighbors };
// Create search parameters.
flann::SearchParams search_params;
// N.B.: We should not be in the boundary case in practice, in which case the
// ambiguity score does not really make sense.
//
// Boundary case 1.
if (keys2.size() == 0)
return;
// Boundary case 2.
if (keys2.size() == 1 && !self_matching)
{
auto m = Match{
&keys1.features[i1], &keys2.features[0], 1.f, dir, int(i1), 0
};
m.rank() = 1;
if(dir == Match::Direction::TargetToSource)
{
swap(m.x_pointer(), m.y_pointer());
swap(m.x_index(), m.y_index());
}
if (m.score() < squared_ratio_thres)
matches.push_back(m);
return;
}
// Boundary case 3.
if (keys2.size() == 2 && self_matching)
{
tree2.knnSearch(query, indices, dists, 2, search_params);
const auto i2 = indices[0][1]; // The first index can't be indices[0][0], which is i1.
auto m = Match{
&keys1.features[i1], &keys2.features[i2], 1.f, dir,
int(i1), i2
};
m.rank() = 1;
if(dir == Match::Direction::TargetToSource)
{
swap(m.x_pointer(), m.y_pointer());
swap(m.x_index(), m.y_index());
}
if (m.score() < squared_ratio_thres)
matches.push_back(m);
return;
}
// Now treat the generic case.
//
// Search the nearest neighbor.
tree2.knnSearch(query, indices, dists, 3, search_params);
// This is to avoid the source key matches with himself in case of intra
// image matching.
const auto top1_index = self_matching ? 1 : 0;
auto top1_score = dists[0][top1_index + 1] > 0.f ?
dists[0][top1_index] / dists[0][top1_index + 1] : 0.f;
auto K = 1;
// Determine the number of nearest neighbors.
if (squared_ratio_thres > 1.f)
{
// Performs an adaptive radius search.
const auto radius = dists[0][top1_index] * squared_ratio_thres;
K = tree2.radiusSearch(query, indices, dists, radius, search_params);
}
// Retrieve the right key points.
for (int rank = top1_index; rank < K; ++rank)
{
auto score = 0.f;
if (rank == top1_index)
score = top1_score;
else if (dists[0][top1_index])
score = dists[0][rank] / dists[0][top1_index];
// We still need this check as FLANN can still return wrong neighbors.
if (score > squared_ratio_thres)
break;
auto i2 = indices[0][rank];
// Ignore the match if keys1 == keys2.
if (self_matching && is_redundant(keys1.features[i1], keys2.features[i2]))
continue;
Match m(&keys1.features[i1], &keys2.features[i2], score, dir, i1, i2);
m.rank() = top1_index == 0 ? rank+1 : rank;
if(dir == Match::Direction::TargetToSource)
{
swap(m.x_pointer(), m.y_pointer());
swap(m.x_index(), m.y_index());
}
matches.push_back(m);
}
}
const IndexParams* getIndexParameters() {
return flann_index->getParameters();
}
int veclen() const {
return flann_index->veclen();
}
void buildIndex() {
flann_index->buildIndex();
}
