HW7 Submission: Feature Matching with RANSAC

R2Image.cpp

std::vector<Feature> R2Image::
Harris(double sigma)
{
  std::cout << "Computing harris filter" << std::endl;
  const R2Image self = *this;
  R2Image *t1 = new R2Image(self); t1->SobelX(); t1->Square();
  R2Image *t2 = new R2Image(self); t2->SobelY(); t2->Square();
  R2Image *t3 = new R2Image(Width(), Height());
  R2Image *t4 = new R2Image(Width(), Height());

  // Set T3 to product of T1 and T2
  for(int x=0; x<Width(); x++) {
    for(int y=0; y<Height(); y++) {
      double v = t1->Pixel(x, y)[0] * t2->Pixel(x, y).Red();
      t3->Pixel(x, y).Reset(v, v, v, 1);
    }
  }

  t1->Blur(2);
  t2->Blur(2);
  t3->Blur(2);

  for(int x=0; x<Width(); x++) {
    for(int y=0; y<Height(); y++) {
      double t1v = t1->Pixel(x, y)[0];
      double t2v = t2->Pixel(x, y)[0];
      double t3v = t3->Pixel(x, y)[0];
      double v = t1v * t2v - t3v * t3v - 0.04 * ((t1v + t2v) * (t1v + t2v));
      v += 0.5;
      t4->Pixel(x, y).Reset(v, v, v, 1);
    }
  }

  std::cout << "Generating feature list" << std::endl;

  std::vector<Feature> features;
  std::vector<Feature> featuresOut;

  for(int x=0; x<Width(); x++) {
    for(int y=0; y<Height(); y++) {
      R2Pixel p = t4->Pixel(x, y);
      double v = p[0];

      double sensitivity = 0.50;

      if(v > sensitivity) {
        features.push_back(Feature(x, y, p));
      }
    }
  }

  std::cout << "Marking best features" << std::endl;

  std::sort(features.begin(), features.end());
  std::reverse(features.begin(), features.end());

  int featuresCount = 150;

  int ct=0, index=0;
  while(ct < featuresCount && index < features.size()) {
    bool skip = false;
    Feature ft = features.at(index);

    for(int i=0; i<index; i++) {
      if(ft.closeTo(features.at(i))) {
        skip = true;
        break; // goes to end of for loop
      }
    }

    if(!skip) {
      featuresOut.push_back(features.at(index));
      ct++;
    }

    index++;
  }

  featuresOut.resize(std::min(int(featuresOut.size()), featuresCount));

  return featuresOut;
}

/**
 * Draws a line from (x1, y1) to (x2, y2) with the RGB color specified by
 * R, G, and B (which are integers from 0 to 255) and draws a box around the
 * point (x2, y2)
 */
void R2Image::
drawLineWithBox(int x1, int y1, int x2, int y2, int r, int g, int b) {
  float rf = float(r)/255.0;
  float gf = float(g)/255.0;
  float bf = float(b)/255.0;

  int m, n, x;

  for(m=-4; m<=4; m++) {
      for(n=-4; n<=4; n++) {
        this->Pixel(x2+m, y2+n).Reset(rf, gf, bf, 1);
      }
    }

    int dx = x2 - x1;
    int dy = y2 - y1;

    if(dx != 0) { // avoid div by zero errors
      for(x=std::min(x1, x2); x<=std::max(x1, x2); x++) {
        int y = int(std::round(y1 + (double(dy * (x-x1)) / double(dx))));
        this->Pixel(x, y).Reset(rf, gf, bf, 1);
      }
    }
}

void R2Image::
blendOtherImageTranslated(R2Image * otherImage)
{
  R2Image *output = new R2Image(*otherImage);
	std::vector<Feature> features = this->Harris(3); // passed by value
  std::vector<Feature>::iterator it;

  int searchSpaceXDim = this->Width() / 10; // half the search space dimension
  int searchSpaceYDim = this->Height() / 10;
  int windowDimension = 12; // half the window dimension

  int count = 0; // temp for cout

  for(it=features.begin(); it != features.end(); it++) {
    std::cout << "feature " << count << " searching..." << std::endl;

    int i, j, m, n;
    double min_ssd = std::numeric_limits<double>::max();
    int min_ssd_x = 0, min_ssd_y = 0;

    Feature ft = *it;

    // Loop through search space

    for(
      i = std::max(ft.centerX - searchSpaceXDim, windowDimension);
      i <= std::min(ft.centerX + searchSpaceXDim, this->Width() - windowDimension);
      i++
    ) {
      for(
        j = std::max(ft.centerY - searchSpaceYDim, windowDimension);
        j <= std::min(ft.centerY + searchSpaceYDim, this->Height() - windowDimension);
        j++
      ) {
        // For each pixel (i, j) in the search space
        double ssd = 0;

        // Calculate the SSD with the feature assuming (i, j) is the center of the new feature
        for(m=-1*windowDimension; m<=windowDimension; m++) {
          for(n=-1*windowDimension; n<=windowDimension; n++) {
            double oldLuminance = this->Pixel(ft.centerX + m, ft.centerY + n).Luminance();
            double newLuminance = otherImage->Pixel(i + m, j + n).Luminance();
            double diff = oldLuminance - newLuminance;
            ssd += diff * diff;
          }
        }

        // If the computed SSD is lower than the current minimum, set the current minimum to (i, j)
        if(ssd < min_ssd) {
          min_ssd = ssd;
          min_ssd_x = i;
          min_ssd_y = j;
        }
      }
    }

    ft.x2 = min_ssd_x;
    ft.y2 = min_ssd_y;
    *it = ft;

    std::cout << "Feature " << count << " at (" << ft.centerX << ", " << ft.centerY << ") found at ("
      << ft.x2 << ", " << ft.y2 << ") with SSD " << min_ssd << std::endl;

    count++; // temp for cout
  }

  std::cout << "Starting RANSAC calculations" << std::endl;

  int numberOfTrials = 10;
  int maxInliers = 0;
  double bestVectorLength = 0;
  double threshold = 3.0;

  for(int i=0; i<numberOfTrials; i++) {

    std::cout << "Starting trial " << i << std::endl;

    // Randomly select a single track
    int randomIndex = rand() % features.size();
    Feature ft = features.at(randomIndex);
    int dx = ft.x2 - ft.centerX;
    int dy = ft.y2 - ft.centerY;
    double vectorLength = sqrt((dx * dx) + (dy * dy));

    std::cout << "Feature " << randomIndex << " at (" << ft.centerX << ", " << ft.centerY << ") found at ("
      << ft.x2 << ", " << ft.y2 << ")" << std::endl;

    // Check all other features, and see if their motion vector is similar
    int inliers = 0;

    for(std::vector<Feature>::iterator it = features.begin(); it != features.end(); it++) {
      int otherdx = it->x2 - it->centerX;
      int otherdy = it->y2 - it->centerY;
      double otherVectorLength = sqrt((otherdx * otherdx) + (otherdy * otherdy));
      double diffVectorLength = abs(vectorLength - otherVectorLength);

      // Count the number of points within a certain distance threshold (inliers)
      if(diffVectorLength < threshold) {
        inliers++;
      }
    }

    // If the number of inliers is less than some threshold repeat the above
    if(inliers > maxInliers) {
      maxInliers = inliers;
      bestVectorLength = vectorLength;
    }
  }

  // After N trials choose the largest inlier set and re-estimate the translation

  // Loop over features and draw output image
  for(std::vector<Feature>::iterator it = features.begin(); it != features.end(); it++) {
    Feature ft = *it;
    int dx = ft.x2 - ft.centerX;
    int dy = ft.y2 - ft.centerY;
    double vectorLength = sqrt((dx * dx) + (dy * dy));

    int r = 0, g = 0, b = 0;

    if(abs(bestVectorLength - vectorLength) < threshold) {
      g = 255;
    } else {
      r = 255;
    }

    output->drawLineWithBox(ft.centerX, ft.centerY, ft.x2, ft.y2, r, g, b);
  }

  this->pixels = output->pixels;
  output->pixels = nullptr;
  delete output;
}