#include <opencv2/imgproc.hpp>
#include <opencv2/features2d.hpp>

static std::vector<cv::Point2f> harrisDetectPoints(cv::Mat& image, const cv::Mat& mask, 
											 int blockSize = 5, int apature = 5, float detectorParameter = 0.01,
											 float minSize = 7, bool verbose = false)
{
	cv::Mat gray;
	cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
	
	//detect corners
	cv::Mat corners;
	cv::cornerHarris(gray, corners, blockSize, apature, detectorParameter);
	cv::normalize(corners, corners, 0, 255, cv::NORM_MINMAX, CV_32FC1, cv::Mat());
	cv::convertScaleAbs( corners, corners );
	cv::threshold(corners, corners, 50, 255, cv::THRESH_BINARY);
	cv::Mat cornersMasked;
	if(mask.data && mask.size == corners.size) corners.copyTo(cornersMasked, mask);
	else corners.copyTo(cornersMasked);
	
	if(verbose)
	{
		cv::imshow( "Viewer", cornersMasked );
		cv::waitKey(0);
	}
	
	//get middle of corners
	cv::SimpleBlobDetector::Params blobParams;
	blobParams.filterByArea = true;
	blobParams.minArea = minSize;
	blobParams.maxArea = 500;
	blobParams.filterByColor = false;
	blobParams.blobColor = 255;
	blobParams.filterByInertia = false;
	blobParams.filterByConvexity = false;
	cv::Ptr<cv::SimpleBlobDetector> blobDetector = cv::SimpleBlobDetector::create(blobParams);
	std::vector<cv::KeyPoint> keypoints;
	blobDetector->detect(cornersMasked, keypoints);
	
	std::vector<cv::Point2f> points;
	cv::KeyPoint::convert(keypoints, points);
	
	return points;
}