/* Compute likely matches between two sets of keypoints */
std::vector<KeypointMatchWithScore>
MatchKeysWithScore(const std::vector<KeypointWithDesc> &k1,
const std::vector<KeypointWithDesc> &k2,
bool registered,
double ratio)
{
int num_pts = 0;
std::vector<KeypointMatchWithScore> matches;
std::vector<KeypointWithDesc> pts;
int *registered_idxs = NULL;
//register为0时，pts保存k2的描述子，每一行是一个描述子
if (!registered) {
pts = k2;
num_pts = (int)k2.size();
}
//registered为1时，registered_idxs保存k2中那些m_extra>=0的点的索引号
else {
registered_idxs = new int[(int)k2.size()];
for (int i = 0; i < (int)k2.size(); i++) {
if (k2[i].m_extra >= 0) {
registered_idxs[num_pts] = i;
pts.push_back(k2[i]);
num_pts++;
}
}
}
for (int i = 0; i < (int)k1.size(); i++) {
int dist[2];
int index = BruteForceMatch(k1[i], pts, dist);
if (sqrt(((double)dist[0]) / ((double)dist[1])) <= ratio) {
if (!registered) {
KeypointMatchWithScore match =
KeypointMatchWithScore(i, index, (float)dist[0]);
matches.push_back(match);
}
else {
KeypointMatchWithScore match =
KeypointMatchWithScore(i, registered_idxs[index],(float)dist[0]);
matches.push_back(match);
}
}
}
int num_matches = (int)matches.size();
printf("[MatchKeysWithScore] Found %d matches\n", num_matches);
if (registered_idxs != NULL) delete registered_idxs;
return matches;
}
/* Prune matches so that they are 1:1 */
std::vector<KeypointMatchWithScore>
PruneMatchesWithScore(const std::vector<KeypointMatchWithScore> &matches)
{
#ifndef WIN32
__gnu_cxx::hash_map<int, float> key_hash;
__gnu_cxx::hash_map<int, int> map;
#else
stdext::hash_map<int, float> key_hash;
stdext::hash_map<int, int> map;
#endif
int num_matches = (int)matches.size();
for (int i = 0; i < num_matches; i++) {
int idx1 = matches[i].m_idx1;
int idx2 = matches[i].m_idx2;
if (key_hash.find(idx2) == key_hash.end()) {
/* Insert the new element */
key_hash[idx2] = matches[i].m_score;
map[idx2] = idx1;
}
else {
float old = key_hash[idx2];
if (old > matches[i].m_score) {
/* Replace the old entry */
key_hash[idx2] = matches[i].m_score;
map[idx2] = idx1;
}
}
}
std::vector<KeypointMatchWithScore> matches_new;
/* Now go through the list again, building a new list */
for (int i = 0; i < num_matches; i++) {
int idx1 = matches[i].m_idx1;
int idx2 = matches[i].m_idx2;
if (map[idx2] == idx1) {
matches_new.push_back(KeypointMatchWithScore(idx1, idx2,
key_hash[idx2]));
}
}
return matches_new;
}
/* Compute likely matches between two sets of keypoints */
std::vector<KeypointMatchWithScore>
MatchKeysWithScore(const std::vector<KeypointWithDesc> &k1,
const std::vector<KeypointWithDesc> &k2,
bool registered,
double ratio)
{
ann_1_1_char::annMaxPtsVisit(200);
int num_pts = 0;
std::vector<KeypointMatchWithScore> matches;
int *registered_idxs = NULL;
if (!registered) {
num_pts = (int) k2.size();
} else {
registered_idxs = new int[(int) k2.size()];
for (int i = 0; i < (int) k2.size(); i++) {
if (k2[i].m_extra >= 0) {
registered_idxs[num_pts] = i;
num_pts++;
}
}
}
/* Create a new array of points */
ann_1_1_char::ANNpointArray pts = ann_1_1_char::annAllocPts(num_pts, 128);
if (!registered) {
for (int i = 0; i < num_pts; i++) {
int j;
for (j = 0; j < 128; j++) {
pts[i][j] = k2[i].m_d[j];
}
}
} else {
for (int i = 0; i < num_pts; i++) {
int j;
int idx = registered_idxs[i];
for (j = 0; j < 128; j++) {
pts[i][j] = k2[idx].m_d[j];
}
}
}
// clock_t start = clock();
/* Create a search tree for k2 */
ann_1_1_char::ANNkd_tree *tree = new ann_1_1_char::ANNkd_tree(pts, num_pts, 128, 4);
// clock_t end = clock();
// printf("Building tree took %0.3fs\n",
// (end - start) / ((double) CLOCKS_PER_SEC));
/* Now do the search */
ann_1_1_char::ANNpoint query = ann_1_1_char::annAllocPt(128);
// start = clock();
for (int i = 0; i < (int) k1.size(); i++) {
int j;
for (j = 0; j < 128; j++) {
query[j] = k1[i].m_d[j];
}
ann_1_1_char::ANNidx nn_idx[2];
ann_1_1_char::ANNdist dist[2];
tree->annkPriSearch(query, 2, nn_idx, dist, 0.0);
if (sqrt(((double) dist[0]) / ((double) dist[1])) <= ratio) {
if (!registered) {
KeypointMatchWithScore match =
KeypointMatchWithScore(i, nn_idx[0], (float) dist[0]);
matches.push_back(match);
} else {
KeypointMatchWithScore match =
KeypointMatchWithScore(i, registered_idxs[nn_idx[0]],
(float) dist[0]);
matches.push_back(match);
}
}
}
// end = clock();
// printf("Searching tree took %0.3fs\n",
// (end - start) / ((double) CLOCKS_PER_SEC));
int num_matches = (int) matches.size();
printf("[MatchKeysWithScore] Found %d matches\n", num_matches);
/* Cleanup */
ann_1_1_char::annDeallocPts(pts);
ann_1_1_char::annDeallocPt(query);
delete tree;
return matches;
}
