object search(ANNkd_tree& kdtree, object q, int k, double eps, bool priority = false)
{
BOOST_ASSERT(k <= kdtree.nPoints() && kdtree.theDim() == len(q));
ANNpointManaged annq(kdtree.theDim());
for (int c = 0; c < kdtree.theDim(); ++c)
annq.pt[c] = extract<ANNcoord>(q[c]);
npy_intp dims[] = { k};
PyObject *pydists = PyArray_SimpleNew(1,dims, sizeof(ANNdist)==8 ? PyArray_DOUBLE : PyArray_FLOAT);
BOOST_ASSERT(!!pydists);
PyObject *pyidx = PyArray_SimpleNew(1,dims, PyArray_INT);
if( !pyidx )
Py_DECREF(pydists);
BOOST_ASSERT(!!pyidx);
ANNdist* pdists = (ANNdist*)PyArray_DATA(pydists);
ANNidx* pidx = (ANNidx*)PyArray_DATA(pyidx);
std::vector<ANNidx> nn_idx(k);
std::vector<ANNdist> dists(k);
if (priority)
kdtree.annkPriSearch(annq.pt, k, pidx, pdists, eps);
else
kdtree.annkSearch(annq.pt, k, pidx, pdists, eps);
return boost::python::make_tuple(static_cast<numeric::array>(handle<>(pyidx)), static_cast<numeric::array>(handle<>(pydists)));
}
object search_array(ANNkd_tree& kdtree, object qarray, int k, double eps, bool priority = false)
{
BOOST_ASSERT(k <= kdtree.nPoints());
int N = len(qarray);
if( N == 0 )
return boost::python::make_tuple(numeric::array(boost::python::list()).astype("i4"),numeric::array(boost::python::list()));
BOOST_ASSERT(len(qarray[0])==kdtree.theDim());
ANNpointManaged annq(kdtree.theDim());
npy_intp dims[] = { N,k};
PyObject *pydists = PyArray_SimpleNew(2,dims, sizeof(ANNdist)==8 ? PyArray_DOUBLE : PyArray_FLOAT);
BOOST_ASSERT(!!pydists);
PyObject *pyidx = PyArray_SimpleNew(2,dims, PyArray_INT);
if( !pyidx ) {
Py_DECREF(pydists);
}
BOOST_ASSERT(!!pyidx);
ANNdist* pdists = (ANNdist*)PyArray_DATA(pydists);
ANNidx* pidx = (ANNidx*)PyArray_DATA(pyidx);
std::vector<ANNdist> dists(k);
std::vector<ANNidx> nn_idx(k);
for(int i = 0; i < N; ++i) {
object q = qarray[i];
for (int c = 0; c < kdtree.theDim(); ++c)
annq.pt[c] = extract<ANNcoord>(q[c]);
if (priority)
kdtree.annkPriSearch(annq.pt, k, &nn_idx[0], &dists[0], eps);
else
kdtree.annkSearch(annq.pt, k, &nn_idx[0], &dists[0], eps);
std::copy(nn_idx.begin(),nn_idx.end(),pidx);
pidx += k;
std::copy(dists.begin(),dists.end(),pdists);
pdists += k;
}
return boost::python::make_tuple(static_cast<numeric::array>(handle<>(pyidx)), static_cast<numeric::array>(handle<>(pydists)));
}
int FeatureMatcher::match() {
matches.clear();
/// For all groups of points clustered based on their epipolar lines
/// Read clusterPointsFast() to see implementation details
for(int i=0; i < pointGroups.size(); i++) {
int qPtSize = pointGroups[i].size();
/// Get corresponding epipolar line for this group of pts
int idx = groupEpiLineIdx[i];
int groupIdx = pointToLineGroupIdx[idx];
/// Get features close to the epipolar line and construct a Kd-tree
/// of its descriptors
vector<int> probMatches;
ANNkd_tree* tree = constructSearchTree(idx, probMatches);
/// Limit the nodes to visit in this tree as max(20% of candidates,20)
int PtsToVisit = (float)(probMatches.size())/20;
PtsToVisit = PtsToVisit > 20 ? PtsToVisit : 20;
annMaxPtsVisit(PtsToVisit);
if(tree == NULL) {
continue;
}
/// For each points within a cluster, find the closest two points
/// from the candidate set (probMatches) using Kd-tree in descriptor
/// space and perform ratio-test
for(int j=0; j < pointGroups[i].size(); j++) {
vector<ANNidx> nn_idx(2);
vector<ANNdist> dists(2);
vector<ANNidx> nn_idx1(2);
vector<ANNdist> dists1(2);
int qPtIdx = pointGroups[i][j];
unsigned char* currQuery = srcKey + 128*qPtIdx;
tree->annkPriSearch(currQuery, 2, nn_idx.data(), dists.data(), 0.0);
/// Perform ratio-test between closest two points
/// Discard the match if ratio is above a threshold
float bestDist = (float)(dists[0]);
float secondBestDist = (float)(dists[1]);
float distRatio1 = sqrt(bestDist/secondBestDist);
if(distRatio1 > 0.6) {
continue;
}
int matchingPt = probMatches[(int)nn_idx[0]];
/// Perform epipolar verification
double x2[] = {refKeysInfo[matchingPt].x,
refKeysInfo[matchingPt].y, 1.0};
double line[3];
geometry::ComputeEpipolarLine(x2,fMatrix.data(),line,true);
double x1[] = {srcKeysInfo[qPtIdx].x,
srcKeysInfo[qPtIdx].y, 1.0};
float epiDist2 =
geometry::ComputeDistanceFromLine( x1, line);
if(epiDist2 >= 4.0) {
continue;
}
matches.push_back(make_pair(qPtIdx, matchingPt));
}
/// Free memory.
annDeallocPts(tree->pts);
delete tree;
}
int matchCount = (int)(matches.size());
return matchCount;
}
int FeatureMatcher::globalMatch(int h, bool twoWaySearch) {
/// Clear previously computed matches if any
matches.clear();
/// Find number of h% matches
int numTopRefPts = (int)(numRefPts*h/100);
int numTopSrcPts = (int)(numSrcPts*h/100);
/// Allocate point structures for Kd-tree based search
ANNpointArray keyPts = annAllocPts( numTopRefPts, 128);
ANNpointArray qKeyPts = annAllocPts( numTopSrcPts, 128);
/// Copy descriptors to allocated point structures
for(int i=0; i < numTopRefPts; i++)
memcpy(keyPts[i], refKey+128*i, sizeof(unsigned char)*128);
for(int i=0; i < numTopSrcPts; i++)
memcpy(qKeyPts[i], srcKey+128*i, sizeof(unsigned char)*128);
/// Create trees for source and target descriptors
ANNkd_tree* tree = new ANNkd_tree(keyPts, numTopRefPts, 128, 16);
ANNkd_tree* qTree = new ANNkd_tree(qKeyPts, numTopSrcPts, 128, 16);
/// Number of nodes to visit in Kd-tree (standard practice)
/// Limit this number to the lesser 500 or TotalPoints/20
/// If the 20% points are too less, limit it at least to 50
int PtsToVisit = numTopRefPts > numTopSrcPts ? numTopRefPts : numTopSrcPts;
PtsToVisit = ceil((float)PtsToVisit/(float)20) <
500 ? ceil((float)PtsToVisit/(float)20) : 500;
if(PtsToVisit < 50) PtsToVisit = 50;
annMaxPtsVisit(PtsToVisit);
//printf("\nPts To Visit : %d", PtsToVisit);
/// For each of the selected source features
for(int i=0; i < numTopSrcPts; i++) {
vector<ANNidx> indices(2);
vector<ANNdist> dists(2);
/// Search for two closest points in the reference tree
unsigned char* qKey = srcKey + 128*i;
tree->annkPriSearch(qKey, 2, indices.data(), dists.data(), 0.0);
/// Compute best distance to second best distance ratio
float bestDist = (float)(dists[0]);
float secondBestDist = (float)(dists[1]);
float distRatio = sqrt(bestDist/secondBestDist);
// printf("\nSrc Point %d, best dist %f, second best %f, ratio %f, 1st %d, 2nd %d",
// i, bestDist, secondBestDist, distRatio, (int)indices[0], (int)indices[1]);
/// If the ratio is larger than threshold, match is not considered
if(distRatio > 0.6) {
continue;
}
/// If the ratio is below the threshold, the closest point is the match
int matchingPt = (int)indices[0];
int secondMatch = (int)indices[1];
indices.clear();
dists.clear();
/// If two way search is enabled, verify that
// the query point is the best match for the
// matching point and also satisfies ratio test
if(twoWaySearch) {
unsigned char* qKey1 = refKey + 128*matchingPt;
qTree->annkPriSearch(qKey1, 2, indices.data(), dists.data(), 0.0);
float bestDist1 = (float)(dists[0]);
float secondBestDist1 = (float)(dists[1]);
float distRatio1 = sqrt(bestDist1/secondBestDist1);
if((int)(indices[0]) != i) {
continue;
}
if(distRatio1 > 0.6) {
continue;
}
}
// printf("\nSrc Point %d, 1st %d, 2nd %d",
// i, matchingPt, secondMatch);
/// Add the pairs to matches list
matches.push_back(make_pair(i, matchingPt));
}
/// Deallocate Point Structures
annDeallocPts(keyPts);
annDeallocPts(qKeyPts);
/// Delete Kd-tree
delete tree;
delete qTree;
return (int)matches.size();
}
