Keypoints matching with SIFT

matching_sift.cpp

#include <opencv2/opencv.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <iostream>
#include <vector>
#include <cmath>

using namespace std;
using namespace cv;

const double THRESHOLD = 400;

/**
 * Calculate euclid distance
 */
double euclidDistance(Mat& vec1, Mat& vec2) {
  double sum = 0.0;
  int dim = vec1.cols;
  for (int i = 0; i < dim; i++) {
    sum += (vec1.at<uchar>(0,i) - vec2.at<uchar>(0,i)) * (vec1.at<uchar>(0,i) - vec2.at<uchar>(0,i));
  }
  return sqrt(sum);
}

/**
 * Find the index of nearest neighbor point from keypoints.
 */
int nearestNeighbor(Mat& vec, vector<KeyPoint>& keypoints, Mat& descriptors) {
  int neighbor = -1;
  double minDist = 1e6;
  
  for (int i = 0; i < descriptors.rows; i++) {
    KeyPoint pt = keypoints[i];
    Mat v = descriptors.row(i);
    double d = euclidDistance(vec, v);
    //printf("%d %f\n", v.cols, d);
    if (d < minDist) {
      minDist = d;
      neighbor = i;
    }
  }
  
  if (minDist < THRESHOLD) {
    return neighbor;
  }
  
  return -1;
}

/**
 * Find pairs of points with the smallest distace between them
 */
void findPairs(vector<KeyPoint>& keypoints1, Mat& descriptors1,
               vector<KeyPoint>& keypoints2, Mat& descriptors2,
               vector<Point2f>& srcPoints, vector<Point2f>& dstPoints) {
  for (int i = 0; i < descriptors1.rows; i++) {
    KeyPoint pt1 = keypoints1[i];
    Mat desc1 = descriptors1.row(i);
    int nn = nearestNeighbor(desc1, keypoints2, descriptors2);
    if (nn >= 0) {
      KeyPoint pt2 = keypoints2[nn];
      srcPoints.push_back(pt1.pt);
      dstPoints.push_back(pt2.pt);
    }
  }
}

int main(int argc, char** argv) {
  if (argc < 2) {
    fprintf(stderr, "Too few arguments\n");
    return -1;
  }
  
  const char* filename = argv[1];

  printf("load file:%s\n", filename);

  // initialize detector and extractor
  FeatureDetector* detector;
  detector = new SiftFeatureDetector(
                                     0, // nFeatures
                                     4, // nOctaveLayers
                                     0.04, // contrastThreshold
                                     10, //edgeThreshold
                                     1.6 //sigma
                                     );

  DescriptorExtractor* extractor;
  extractor = new SiftDescriptorExtractor();

  // Compute keypoints and descriptor from the source image in advance
  vector<KeyPoint> keypoints2;
  Mat descriptors2;

  Mat originalGrayImage = imread(filename, CV_LOAD_IMAGE_GRAYSCALE);
  if (!originalGrayImage.data) {
    printf("gray image load error\n");
    exit(1);
  }
  Mat originalColorImage = imread(filename, CV_LOAD_IMAGE_ANYCOLOR|CV_LOAD_IMAGE_ANYDEPTH);
  if (!originalColorImage.data) {
    printf("color image open error\n");
    exit(1);
  }

  detector->detect(originalGrayImage, keypoints2);
  extractor->compute(originalGrayImage, keypoints2, descriptors2);
  printf("original image:%d keypoints are found.\n", (int)keypoints2.size());

  VideoCapture capture(0);
  capture.set(CV_CAP_PROP_FRAME_WIDTH,640);
  capture.set(CV_CAP_PROP_FRAME_HEIGHT,480);

  namedWindow("matches");
  Mat frame;
  int c;
  int fileIndex = 0;
  while (1) {
    capture >> frame;

    // load gray scale image from camera
    Size size = frame.size();
    Mat grayFrame(size, CV_8UC1);
    cvtColor(frame, grayFrame, CV_BGR2GRAY);
    if (!grayFrame.data) {
        cerr << "cannot find image file1" << endl;
        exit(-1);
    }

    // Create a image for displaying mathing keypoints
    Size sz = Size(size.width + originalColorImage.size().width, size.height + originalColorImage.size().height);
    Mat matchingImage = Mat::zeros(sz, CV_8UC3);

    // Draw camera frame
    Mat roi1 = Mat(matchingImage, Rect(0, 0, size.width, size.height));
    frame.copyTo(roi1);
    // Draw original image
    Mat roi2 = Mat(matchingImage, Rect(size.width, size.height, originalColorImage.size().width, originalColorImage.size().height));
    originalColorImage.copyTo(roi2);

    vector<KeyPoint> keypoints1;
    Mat descriptors1;
    vector<DMatch> matches;

    // Detect keypoints
    detector->detect(grayFrame, keypoints1);
    extractor->compute(grayFrame, keypoints1, descriptors1);
    
    printf("image1:%zd keypoints are found.\n", keypoints1.size());

    for (int i=0; i<keypoints1.size(); i++){
      KeyPoint kp = keypoints1[i];
      circle(matchingImage, kp.pt, cvRound(kp.size*0.25), Scalar(255,255,0), 1, 8, 0);
    }
    // Find nearest neighbor pairs
    vector<Point2f> srcPoints;
    vector<Point2f> dstPoints;
    findPairs(keypoints1, descriptors1, keypoints2, descriptors2, srcPoints, dstPoints);
    printf("%zd keypoints are matched.\n", srcPoints.size());

    char text[256];
    sprintf(text, "%zd/%zd keypoints matched.", srcPoints.size(), keypoints2.size());
    putText(matchingImage, text, Point(0, cvRound(size.height + 30)), FONT_HERSHEY_SCRIPT_SIMPLEX, 1, Scalar(0,0,255));

    // Draw line between nearest neighbor pairs
    for (int i = 0; i < (int)srcPoints.size(); ++i) {
      Point2f pt1 = srcPoints[i];
      Point2f pt2 = dstPoints[i];
      Point2f from = pt1;
      Point2f to   = Point(size.width + pt2.x, size.height + pt2.y);
      line(matchingImage, from, to, Scalar(0, 255, 255));
    }

    // Display mathing image
    imshow("matches", matchingImage);

    c = waitKey(2);
    if (c == '\x1b')
      break;
  }

  return 0;
}