HW10 Submission: Blended Transformed Images

R2Image.cpp

std::vector<Feature> R2Image::
Harris(double sigma)
{
  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::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::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;
}


void R2Image::
Sharpen()
{
  R2Image *output = new R2Image(width, height);

  double sharpen[3][3] = {
    {-2, -2, -2},
    {-2, 16, -2},
    {-2, -2, -2}
  };

  for (int i = 1; i < width-1; i++) {
    for (int j = 1;  j < height-1; j++) {

      double sharpenR =
        (sharpen[0][0] * Pixel(i-1,j-1).Red()) + (sharpen[0][1] * Pixel(i,j-1).Red()) + (sharpen[0][2] * Pixel(i+1,j-1).Red()) +
        (sharpen[1][0] * Pixel(i-1,j).Red())   + (sharpen[1][1] * Pixel(i,j).Red())   + (sharpen[1][2] * Pixel(i+1,j).Red()) +
        (sharpen[2][0] * Pixel(i-1,j+1).Red()) + (sharpen[2][1] * Pixel(i,j+1).Red()) + (sharpen[2][2] * Pixel(i+1,j+1).Red());

      double sharpenG =
        (sharpen[0][0] * Pixel(i-1,j-1).Green()) + (sharpen[0][1] * Pixel(i,j-1).Green()) + (sharpen[0][2] * Pixel(i+1,j-1).Green()) +
        (sharpen[1][0] * Pixel(i-1,j).Green())   + (sharpen[1][1] * Pixel(i,j).Green())   + (sharpen[1][2] * Pixel(i+1,j).Green()) +
        (sharpen[2][0] * Pixel(i-1,j+1).Green()) + (sharpen[2][1] * Pixel(i,j+1).Green()) + (sharpen[2][2] * Pixel(i+1,j+1).Green());

      double sharpenB =
        (sharpen[0][0] * Pixel(i-1,j-1).Blue()) + (sharpen[0][1] * Pixel(i,j-1).Blue()) + (sharpen[0][2] * Pixel(i+1,j-1).Blue()) +
        (sharpen[1][0] * Pixel(i-1,j).Blue())   + (sharpen[1][1] * Pixel(i,j).Blue())   + (sharpen[1][2] * Pixel(i+1,j).Blue()) +
        (sharpen[2][0] * Pixel(i-1,j+1).Blue()) + (sharpen[2][1] * Pixel(i,j+1).Blue()) + (sharpen[2][2] * Pixel(i+1,j+1).Blue());

      R2Pixel *newPixel = new R2Pixel(Pixel(i, j).Red() + sharpenR / 2,
        Pixel(i, j).Green() + sharpenG / 2, Pixel(i, j).Blue() + sharpenB / 2, 1);
      newPixel->Clamp();
      output->SetPixel(i, j, *newPixel);
    }
  }

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

/**
 * 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 computeInverseMatrix(double *i, double *output) {
  /** Algorithm based on publically available algorithm found here:
   *  https://forgetcode.com/C/1781-Inverse-Matrix-of-3x3#
   */
  double a[3][3] = {{i[0],i[1],i[2]},{i[3],i[4],i[5]},{i[6],i[7],i[8]}};
  double determinant = 0;

  for(int i=0;i<3;i++) {
    determinant = determinant + (a[0][i]*(a[1][(i+1)%3]*a[2][(i+2)%3] - a[1][(i+2)%3]*a[2][(i+1)%3]));
  }

  for(int i=0;i<3;i++){
    for(int j=0;j<3;j++) {
      output[3*j+i] = (((a[(i+1)%3][(j+1)%3] * a[(i+2)%3][(j+2)%3]) - (a[(i+1)%3][(j+2)%3]*a[(i+2)%3][(j+1)%3]))/ determinant);
    }
  }
}

void computeHomographyMatrix(std::vector<PointCorrespondence> correspondences, double *k) {
    if(correspondences.size() < 4) {
        return;
    }

    int numberOfRows = correspondences.size() * 2;
    double **a = dmatrix(1, numberOfRows, 1, 9);

    for(int i=0; i<correspondences.size(); i++) {
      double x = correspondences.at(i).before.x;
      double y = correspondences.at(i).before.y;
      double u = correspondences.at(i).after.x;
      double v = correspondences.at(i).after.y;

      a[2*i+1][1] = -1 * x;
      a[2*i+1][2] = -1 * y;
      a[2*i+1][3] = -1;
      a[2*i+1][4] = 0;
      a[2*i+1][5] = 0;
      a[2*i+1][6] = 0;
      a[2*i+1][7] = u * x;
      a[2*i+1][8] = u * y;
      a[2*i+1][9] = u;
      a[2*i+2][1] = 0;
      a[2*i+2][2] = 0;
      a[2*i+2][3] = 0;
      a[2*i+2][4] = -1 * x;
      a[2*i+2][5] = -1 * y;
      a[2*i+2][6] = -1;
      a[2*i+2][7] = v * x;
      a[2*i+2][8] = v * y;
      a[2*i+2][9] = v;
    }

    double w[10]; // 1..9

    double **v = dmatrix(1, 9, 1, 9);

    svdcmp(a, numberOfRows, 9, w, v);

    // find the smallest singular value:
    int mi = 1;
    for(int i=2;i<=9;i++) if(w[i]<w[mi]) mi=i;

    // solution is the nullspace of the matrix, which is the column in V corresponding to the smallest singular value (which should be 0)
    for(int i=1;i<=9;i++) k[i-1]=v[i][mi];
}

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

  for(it=features.begin(); it != features.end(); it++) {
    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;
  }

  int numberOfTrials = 3000;
  int maxInliers = 0;
  std::vector<int> inlierIndices;
  double* bestK = nullptr;
  double threshold = 8.0;

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

    std::vector<int> tempInlierIndices;

    // Randomly select a single track
    int randomIndices[] = {rand() % features.size(), rand() % features.size(), rand() % features.size(), rand() % features.size()};
    std::vector<PointCorrespondence> correspondences;
    correspondences.push_back(createCorrespondence(features.at(randomIndices[0])));
    correspondences.push_back(createCorrespondence(features.at(randomIndices[1])));
    correspondences.push_back(createCorrespondence(features.at(randomIndices[2])));
    correspondences.push_back(createCorrespondence(features.at(randomIndices[3])));

    double *k = (double *) malloc(sizeof(double) * 9);
    computeHomographyMatrix(correspondences, k);

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

    for(int i=0; i<features.size(); i++) {
      Feature ft = features.at(i);
      double ha_x, ha_y, ha_z;

      // matrix multiplication
      ha_x = ft.centerX * k[0] + ft.centerY * k[1] + 1*k[2];
      ha_y = ft.centerX * k[3] + ft.centerY * k[4] + 1*k[5];
      ha_z = ft.centerX * k[6] + ft.centerY * k[7] + 1*k[8];

      // normalization
      ha_x /= ha_z;
      ha_y /= ha_z;

      double diffVectorLength = abs(sqrt((ha_x-ft.x2)*(ha_x-ft.x2)+(ha_y-ft.y2)*(ha_y-ft.y2)));

      // Count the number of points whose feature match is within a distance
      // threshold of the original point translated by the translation matrix
      if(diffVectorLength < threshold) {
        tempInlierIndices.push_back(i);
        inliers++;
      }
    }

    // If the number of inliers is less than some threshold repeat the above
    if(inliers > maxInliers) {
      maxInliers = inliers;
      bestK = k;
      inlierIndices = tempInlierIndices;
    }
  }

  std::vector<PointCorrespondence> bestCorr;
  for(int i=0; i<inlierIndices.size(); i++) {
    bestCorr.push_back(createCorrespondence(features.at(inlierIndices.at(i))));
  }

  double *k = (double *) malloc(sizeof(double) * 9);
  double *invk = (double *) malloc(sizeof(double) * 9);
  computeHomographyMatrix(bestCorr, k);
  computeInverseMatrix(k, invk);

  for(int i=0; i<Width(); i++) {
    for(int j=0; j<Height(); j++) {
      // matrix multiplication
      double inv_x = i * invk[0] + j * invk[1] + invk[2];
      double inv_y = i * invk[3] + j * invk[4] + invk[5];
      double inv_z = i * invk[6] + j * invk[7] + invk[8];

      // normalization
      inv_x /= inv_z;
      inv_y /= inv_z;

      double floating_x = inv_x - floor(inv_x);
      double floating_y = inv_y - floor(inv_y);

      if(inv_x < 0 || inv_x > Width() || inv_y < 0 || inv_y > Height()) {
        continue;
      }

      double r =
        (Pixel(floor(inv_x), floor(inv_y)).Red() * floating_x +
        Pixel(ceil(inv_x),  floor(inv_y)).Red() * (1.0 - floating_x)) * floating_y +
        (Pixel(floor(inv_x), ceil(inv_y)).Red() * floating_x +
        Pixel(ceil(inv_x),  ceil(inv_y)).Red() * (1.0 - floating_x)) * (1.0 - floating_y);

      double g =
        (Pixel(floor(inv_x), floor(inv_y)).Green() * floating_x +
        Pixel(ceil(inv_x),  floor(inv_y)).Green() * (1.0 - floating_x)) * floating_y +
        (Pixel(floor(inv_x), ceil(inv_y)).Green() * floating_x +
        Pixel(ceil(inv_x),  ceil(inv_y)).Green() * (1.0 - floating_x)) * (1.0 - floating_y);

      double b =
        (Pixel(floor(inv_x), floor(inv_y)).Blue() * floating_x +
        Pixel(ceil(inv_x),  floor(inv_y)).Blue() * (1.0 - floating_x)) * floating_y +
        (Pixel(floor(inv_x), ceil(inv_y)).Blue() * floating_x +
        Pixel(ceil(inv_x),  ceil(inv_y)).Blue() * (1.0 - floating_x)) * (1.0 - floating_y);

      bool debug = true;

      if(debug) {
        r += 0.25;
        g += 0.25;
        b += 0.25;
      }

      output->Pixel(i, j).SetRed(output->Pixel(i, j).Red() * 0.5 + r * 0.5);
      output->Pixel(i, j).SetGreen(output->Pixel(i, j).Green() * 0.5 + g * 0.5);
      output->Pixel(i, j).SetBlue(output->Pixel(i, j).Blue() * 0.5 + b * 0.5);
    }
  }

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