From 3830c1e411134dafbfe8a5b3a70ceb251593e0d7 Mon Sep 17 00:00:00 2001
From: lhark <lhark@hotmail.fr>
Date: Mon, 16 May 2016 00:27:55 +0200
Subject: [PATCH] Implement Kyle Hounslow's greyscale threshold motion tracking
 example

---
 src/papillon.cpp | 97 ++++++++++++++++++++++++++++++++++++------------
 1 file changed, 74 insertions(+), 23 deletions(-)

diff --git a/src/papillon.cpp b/src/papillon.cpp
index aea0058..7e1e71a 100644
--- a/src/papillon.cpp
+++ b/src/papillon.cpp
@@ -12,9 +12,15 @@ using namespace std;
 
 class Traite_image {
 	public:
+		const static int SENSITIVITY_VALUE = 30;
+		const static int BLUR_SIZE = 10;
+
 		Mat prev;
 		bool first = true;
-		int resize_f = 8;
+		int resize_f = 1;
+
+		int theObject[2] = {0,0};
+		Rect objectBoundingRectangle = Rect(0,0,0,0);
 
 		ros::NodeHandle n;
 
@@ -47,42 +53,87 @@ class Traite_image {
 			Mat next;
 			resize(input, next, Size(input.size().width/resize_f, input.size().height/resize_f));
 			cvtColor(next, next, CV_BGR2GRAY);
-			Mat output; //   (input.rows, input.cols, CV_32FC2);
-			ROS_INFO("got input");
+			Mat output = input.clone(); //   (input.rows, input.cols, CV_32FC2);
+			//ROS_INFO("got input");
 			if (first) {
 				prev = next.clone();
 				first = false;
 				ROS_INFO("first done");
 			}
 
-			//unsigned int size           = input.rows * input.cols * 3;
-			//unsigned char* begin_input  = (unsigned char*)(input.data);
-			//unsigned char* end_input    = (unsigned char*)(input.data) + size;
-			//unsigned char* out          = (unsigned char*)(output.data);
-			//unsigned char* in           = begin_input;
-
-			// This is an efficient way to process each channel in each pixel,
-			// with an iterator taste.
-			//while(in != end_input) {
-			//  *(out++) = *(ptr_prev) - *(in);
-			//  *(ptr_prev++) = *(in++);
-			//}
-
-			Mat_<Point2f> flow;
-			Ptr<DenseOpticalFlow> tvl1 = createOptFlow_DualTVL1();
-
-			tvl1->calc(prev, next, flow);
-
-			drawOpticalFlow(flow, output);
+			// Subtract the 2 last frames and threshold them
+			Mat thres;
+			absdiff(prev,next,thres);
+			threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
+			// Blur to eliminate noise
+			blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE));
+			threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
+			searchForMovement(thres, output);
 
 			pub.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
 			// bridge_input is handled by a smart-pointer. No explicit delete needed.
 
-			ROS_INFO("pub");
+			//ROS_INFO("pub");
 
 			prev = next.clone();
 		}
 
+		//int to string helper function
+		string intToString(int number){
+
+			//this function has a number input and string output
+			std::stringstream ss;
+			ss << number;
+			return ss.str();
+		}
+
+		void searchForMovement(Mat thresholdImage, Mat &cameraFeed){
+			//notice how we use the '&' operator for objectDetected and cameraFeed. This is because we wish
+			//to take the values passed into the function and manipulate them, rather than just working with a copy.
+			//eg. we draw to the cameraFeed to be displayed in the main() function.
+			bool objectDetected = false;
+			Mat temp;
+			thresholdImage.copyTo(temp);
+			//these two vectors needed for output of findContours
+			vector< vector<Point> > contours;
+			vector<Vec4i> hierarchy;
+			//find contours of filtered image using openCV findContours function
+			//findContours(temp,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE );// retrieves all contours
+			findContours(temp,contours,hierarchy,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE );// retrieves external contours
+
+			//if contours vector is not empty, we have found some objects
+			if(contours.size()>0)objectDetected=true;
+			else objectDetected = false;
+
+			if(objectDetected){
+				//the largest contour is found at the end of the contours vector
+				//we will simply assume that the biggest contour is the object we are looking for.
+				vector< vector<Point> > largestContourVec;
+				largestContourVec.push_back(contours.at(contours.size()-1));
+				//make a bounding rectangle around the largest contour then find its centroid
+				//this will be the object's final estimated position.
+				objectBoundingRectangle = boundingRect(largestContourVec.at(0));
+				int xpos = objectBoundingRectangle.x+objectBoundingRectangle.width/2;
+				int ypos = objectBoundingRectangle.y+objectBoundingRectangle.height/2;
+
+				//update the objects positions by changing the 'theObject' array values
+				theObject[0] = xpos , theObject[1] = ypos;
+			}
+			//make some temp x and y variables so we dont have to type out so much
+			int x = theObject[0];
+			int y = theObject[1];
+
+			//draw some crosshairs around the object
+			circle(cameraFeed,Point(x,y),20,Scalar(0,255,0),2);
+			line(cameraFeed,Point(x,y),Point(x,y-25),Scalar(0,255,0),2);
+			line(cameraFeed,Point(x,y),Point(x,y+25),Scalar(0,255,0),2);
+			line(cameraFeed,Point(x,y),Point(x-25,y),Scalar(0,255,0),2);
+			line(cameraFeed,Point(x,y),Point(x+25,y),Scalar(0,255,0),2);
+
+			//write the position of the object to the screen
+			putText(cameraFeed,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2);
+		}
+
 		inline bool isFlowCorrect(Point2f u)
 		{
 			return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;