Notes

//Image Credits: http://www.cs.cmu.edu/afs/cs.cmu.edu/academic/class/15463-f08/www/proj4/www/dmillett/

#include <stdio.h>
#include <iostream>
#include "opencv2/core/core.hpp"
using namespace std;
//Order for include and using namespace is crucial
using namespace cv;
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "harrisDetector.h"

#define OPENCV_OLD
//#define OPENCV_NEW

//Corners
int ksize=2;
int threshold_value=1;

//GFTT
int max_corners = 500; // Maximum number of corners to be returned
int quality_level = 10; // Quality Level
int min_distance = 10; // Minimum allowed distance between points

//FAST
int fast_threshold_value=40;

float precision = 0.001f;

const std::string trackbarWindowName1 = "Corners";
const std::string trackbarWindowName2 = "GFTT";
const std::string trackbarWindowName3 = "FAST";

void on_trackbar( int, void* )
{//This function gets called whenever a
// trackbar position is changed
}

void createTrackbars1(){
//Create Window for Trackbars
namedWindow(trackbarWindowName1,0);
//Create memory to store trackbar name on window
char TrackbarName[50];

sprintf( TrackbarName, "Kernel Size");
sprintf( TrackbarName, "Threshold Value");
createTrackbar( "Kernel Size", trackbarWindowName1, &ksize, 6, on_trackbar );
createTrackbar( "Threshold Value", trackbarWindowName1, &threshold_value, 255, on_trackbar );
}

void createTrackbars2(){
//Create window for trackbars
namedWindow(trackbarWindowName2,0);
//create memory to store trackbar name on window
char TrackbarName[50];
sprintf( TrackbarName, "Max Corners");
sprintf( TrackbarName, "Quality Level");
sprintf( TrackbarName, "Min Distance");
createTrackbar("Max Corners", trackbarWindowName2, &max_corners, 1000, on_trackbar );
createTrackbar("Quality Level", trackbarWindowName2, &quality_level, 50, on_trackbar );
createTrackbar("Min Distance", trackbarWindowName2, &min_distance, 50, on_trackbar );
}

void createTrackbars3(){
//Create Window for Trackbars
namedWindow(trackbarWindowName3,0);
//Create memory to store trackbar name on window
char TrackbarName[50];

sprintf( TrackbarName, "Threshold Value");
createTrackbar( "Threshold Value", trackbarWindowName3, &fast_threshold_value, 255, on_trackbar );
}

void change_resolution(VideoCapture* cap,int width,int height){
cap->set(CV_CAP_PROP_FRAME_WIDTH, width);
cap->set(CV_CAP_PROP_FRAME_HEIGHT,height);
}

void resize_frame(Mat* frame,int width,int height){
resize(*frame, *frame, Size(width,height), 0, 0, INTER_CUBIC);
}


int main( int argc, char** argv )
{
//initModule_nonfree();
if( argc != 3)
{
cout <<" Usage: Interest_Points [image]" << endl;
return -1;
}

Mat image1,image2;
image1 = imread(argv[1], CV_LOAD_IMAGE_COLOR); // Read the file
image2 = imread(argv[2], CV_LOAD_IMAGE_COLOR); // Read the file
//VideoCapture cap("../data/left/video2_denoised.avi");

//if(!cap.isOpened())
if(!image1.data || !image2.data ){ // Check for invalid input
cout << "Could not open or find the image!" << std::endl ;
return -1;
}

// while(1){
//cap >> image;

//resize_frame(&image,640,480);

namedWindow("Image1",WINDOW_AUTOSIZE); // Create a window for display.
namedWindow("Image2",WINDOW_AUTOSIZE); // Create a window for display.
imshow("Image1",image1); // Show our image inside it.
imshow("Image2",image2); // Show our image inside it.

// // Convert source image to gray
// Mat imageG,dst;
// dst = Mat::zeros(image.size(),CV_32FC1);
// cvtColor(image,imageG,CV_BGR2GRAY );
// //imshow("Grey",imageG);

// // Detecting Corners
// cornerHarris( imageG, dst, 7, 2*ksize+1, 0.05, BORDER_DEFAULT);
// //cornerHarris(grey,dst,3,3,0.01);
//
// // Threshold the Corner Strengths
// cv::Mat harrisCorners;
// //cout << threshold_value*precision << endl;
// cv::threshold(dst,harrisCorners,threshold_value*precision,255,cv::THRESH_BINARY_INV);
// imshow( "Corners", harrisCorners ); // Show our image inside it.
//
// // Applying Sobel Filter
// Mat sobelx;
// Sobel(imageG, sobelx, CV_32F, 1, 0);
// double minVal, maxVal;
// minMaxLoc(sobelx, &minVal, &maxVal); //find minimum and maximum intensities
// //cout << "minVal : " << minVal << endl << "maxVal : " << maxVal << endl;
//
// Mat sobel;
// sobelx.convertTo(sobel, CV_8U, 255.0/(maxVal - minVal), -minVal * 255.0/(maxVal - minVal));
// //namedWindow("Sobel", CV_WINDOW_AUTOSIZE);
// //imshow("Sobel", sobel);
//// Detecting Corners
// cornerHarris( imageG, dst, 7, 2*ksize+1, 0.05, BORDER_DEFAULT);
// //cornerHarris(grey,dst,3,3,0.01);
//
// // Threshold the Corner Strengths
// cv::Mat harrisCorners;
// //cout << threshold_value*precision << endl;
// cv::threshold(dst,harrisCorners,threshold_value*precision,255,cv::THRESH_BINARY_INV);
// imshow( "Corners", harrisCorners ); // Show our image inside it.
//
// // Applying Sobel Filter
// Mat sobelx;
// Sobel(imageG, sobelx, CV_32F, 1, 0);
// double minVal, maxVal;
// minMaxLoc(sobelx, &minVal, &maxVal); //find minimum and maximum intensities
// //cout << "minVal : " << minVal << endl << "maxVal : " << maxVal << endl;
//
// Mat sobel;
// sobelx.convertTo(sobel, CV_8U, 255.0/(maxVal - minVal), -minVal * 255.0/(maxVal - minVal));
// //namedWindow("Sobel", CV_WINDOW_AUTOSIZE);
// //imshow("Sobel", sobel);
//
// // Create Harris detector instance
// HarrisDetector harris;
// // Compute Harris values
// harris.detect(dst);
// //imshow("1",dst);
//
// // Detect Harris corners
// std::vector<cv::Point> pts;
// harris.getCorners(pts,0.000002);
// // Draw Harris corners
// Mat draw;
// image.copyTo(draw);
// harris.drawOnImage(draw,pts);
// //create slider bars for HSV filtering
// //imshow("Harris Corner Detector", draw); // Show our image inside it.
// createTrackbars1();
//
// Mat dilated;
// cv::dilate(draw,dilated,cv::Mat());
// //imshow( "Dilated", dilated); // Show our image inside it.
// Mat localMax;
// cv::compare(draw,dilated,localMax,cv::CMP_EQ);
// //imshow("localMax",localMax);
//
// // Compute good features to track
// std::vector<cv::Point2f> corners;
// //const int max_corners = 500; // Maximum number of corners to be returned
// //double quality_level = 0.01; // Quality Level
// //double min_distance = 10; // Minimum allowed distance between points
// //int use_harris = false;
// //cout << quality_level*precision << endl;
// cv::goodFeaturesToTrack(imageG,corners,max_corners,(quality_level+1)*10*precision,min_distance);
//
// //GFTT Method
// Mat GFTT;
// int radius = 7;
// image.copyTo(GFTT);
// for( int i = 0; i < max_corners; i++){
// circle(GFTT,cvPoint((int)(corners[i].x + 0.5f),(int)(corners[i].y + 0.5f)),radius,cv::Scalar(255,255,255));
// }
// cv::namedWindow("GFTT");
// imshow("GFTT", GFTT);
// createTrackbars2();
// // Create Harris detector instance
// HarrisDetector harris;
// // Compute Harris values
// harris.detect(dst);
// //imshow("1",dst);
//
// // Detect Harris corners
// std::vector<cv::Point> pts;
// harris.getCorners(pts,0.000002);
// // Draw Harris corners
// Mat draw;
// image.copyTo(draw);
// harris.drawOnImage(draw,pts);
// //create slider bars for HSV filtering
// //imshow("Harris Corner Detector", draw); // Show our image inside it.
// createTrackbars1();
//
// Mat dilated;
// cv::dilate(draw,dilated,cv::Mat());
// //imshow( "Dilated", dilated); // Show our image inside it.
// Mat localMax;
// cv::compare(draw,dilated,localMax,cv::CMP_EQ);
// //imshow("localMax",localMax);
//
// // Compute good features to track
// std::vector<cv::Point2f> corners;
// //const int max_corners = 500; // Maximum number of corners to be returned
// //double quality_level = 0.01; // Quality Level
// //double min_distance = 10; // Minimum allowed distance between points
// //int use_harris = false;
// //cout << quality_level*precision << endl;
// cv::goodFeaturesToTrack(imageG,corners,max_corners,(quality_level+1)*10*precision,min_distance);
//
// //GFTT Method
// Mat GFTT;
// int radius = 7;
// image.copyTo(GFTT);
// for( int i = 0; i < max_corners; i++){
// circle(GFTT,cvPoint((int)(corners[i].x + 0.5f),(int)(corners[i].y + 0.5f)),radius,cv::Scalar(255,255,255));
// }
// cv::namedWindow("GFTT");
// imshow("GFTT", GFTT);
// createTrackbars2();

//FAST Method
std::vector<cv::KeyPoint> keypoints1;
std::vector<cv::KeyPoint> keypoints2;
Mat fast1,fast2;
image1.copyTo(fast1);
image2.copyTo(fast2);
// Construction of the Fast feature detector object
#ifdef OPENCV_NEW
//Ptr<FastFeatureDetector> FAST = FastFeatureDetector::create(fast_threshold_value);
Ptr<FastFeatureDetector> FAST = FastFeatureDetector::create(120);
FAST->detect(fast1,keypoints1);
FAST->detect(fast2,keypoints2);
#endif

#ifdef OPENCV_OLD
//FastFeatureDetector FAST(fast_threshold_value);
FastFeatureDetector FAST(120);
FAST.detect(fast1,keypoints1);
FAST.detect(fast2,keypoints2);
#endif

//createTrackbars3();

cv::drawKeypoints(fast1,keypoints1,fast1,cv::Scalar(255,255,255),cv::DrawMatchesFlags::DRAW_OVER_OUTIMG);
cv::drawKeypoints(fast2,keypoints2,fast2,cv::Scalar(255,255,255),cv::DrawMatchesFlags::DRAW_OVER_OUTIMG);
cv::namedWindow("FAST1");
cv::namedWindow("FAST2");
cv::imshow("FAST1",fast1);
cv::imshow("FAST2",fast2);

// Define a square neighborhood
const int nsize(11); // size of the neighborhood
cv::Rect neighborhood(0, 0, nsize, nsize); // 11x11
cv::Mat patch1;
cv::Mat patch2;

// For all keypoints in first image
// find best match in second image
cv::Mat result;
std::vector<cv::DMatch> matches;
//for all keypoints in image 1
for (unsigned int i=0; i<keypoints1.size(); i++) {
// define image patch
neighborhood.x = keypoints1[i].pt.x-nsize/2;
neighborhood.y = keypoints1[i].pt.y-nsize/2;

// if neighborhood of points outside image,
// then continue with next point
if (neighborhood.x<0 || neighborhood.y<0 || neighborhood.x+nsize >= image1.cols || neighborhood.y+nsize >= image1.rows)
continue;

//patch in image 1
patch1 = image1(neighborhood);

// reset best correlation value;
cv::DMatch bestMatch;

//for all keypoints in image 2
for (unsigned int j=0; j<keypoints2.size(); j++) {
// define image patch
neighborhood.x = keypoints2[j].pt.x-nsize/2;
neighborhood.y = keypoints2[j].pt.y-nsize/2;

// if neighborhood of points outside image,
// then continue with next point
if (neighborhood.x<0 || neighborhood.y<0 || neighborhood.x + nsize >= image2.cols || neighborhood.y + nsize >= image2.rows)
continue;

// patch in image 2
patch2 = image2(neighborhood);

// match the two patches
cv::matchTemplate(patch1,patch2,result,CV_TM_SQDIFF_NORMED);

// check if it is a best match
if (result.at<float>(0,0) < bestMatch.distance) {
bestMatch.distance= result.at<float>(0,0);
bestMatch.queryIdx= i;
bestMatch.trainIdx= j;
}
}

// add the best match
matches.push_back(bestMatch);
}

// Extract the N best matches
int N=7;
std::nth_element(matches.begin(),matches.begin()+N,matches.end());
matches.erase(matches.begin()+N,matches.end());

// Draw the matching results
Mat matchImage;
cv::drawMatches(image1,keypoints1,image2,keypoints2,matches,matchImage,cv::Scalar(255,255,255),cv::Scalar(255,255,255));
imshow("Matches",matchImage);

//Calculating Fundamental Matrix
//The minimum number of such matches is seven. It regards Correspondence problem.
// Convert keypoints into Point2f
int point_count = 7;
std::vector<cv::Point2f> selPoints1; //7 points in first image
std::vector<cv::Point2f> selPoints2; //7 points in second image
//std::vector<cv::Point2f> selPoints1_resize; //7 points in first image
//std::vector<cv::Point2f> selPoints2_resize; //7 points in second image
std::vector<int> pointIndexes1, pointIndexes2;
cv::KeyPoint::convert(keypoints1,selPoints1,pointIndexes1);
cv::KeyPoint::convert(keypoints2,selPoints2,pointIndexes2);

// // Initialize the points here ... */
// for( int i = 0; i < point_count; i++ )
// {
// selPoints1_resize[i] = selPoints1[i];
// selPoints1_resize[i] = selPoints1[i];
// }

cout << "selPoints1:n" << selPoints1 << endl;
cout << "selPoints2:n" << selPoints2 << endl;

if(selPoints1.size() == selPoints2.size()){
cout << "Oi1" << endl;
}else{
cout << "Different Sizes!" << endl;
cout << "selPoints1 Size: "<< selPoints1.size() << endl;
cout << "selPoints2 Size: "<< selPoints2.size() << endl;
}

// Compute F matrix from 7 matches
cv::Mat F=findFundamentalMat(selPoints1,selPoints2,CV_FM_7POINT); // 7-Point Method
//cv::Mat F=findFundamentalMat(selPoints1,selPoints2,CV_FM_8POINT); // 8-Point Method

// char key = waitKey(1);
// if(key=='q'){
// break;
// }
// }
waitKey(0);
return 0;
}