From ca53673111ad4ea1de814716c7b7b3e6dc770fad Mon Sep 17 00:00:00 2001
From: samilyjcc <lejcdu84@hotmail.fr>
Date: Tue, 14 Jun 2016 11:36:58 +0200
Subject: [PATCH] add close feature

---
 src/papillon.cpp | 51 +++++++++++++++++++++++++++++++++---------------
 1 file changed, 35 insertions(+), 16 deletions(-)

diff --git a/src/papillon.cpp b/src/papillon.cpp
index f563ae8..fcb20e6 100644
--- a/src/papillon.cpp
+++ b/src/papillon.cpp
@@ -14,7 +14,7 @@ using namespace std;
 class Traite_image {
 	public:
 		const static int SENSITIVITY_VALUE = 40;
-		const static int BLUR_SIZE = 10;
+		const static int BLUR_SIZE = 15;
 
 
 		Mat prev;
@@ -78,7 +78,7 @@ class Traite_image {
 			searchForMovement(prev_cropped, next_stab_cropped, output);
 
 
-			pub_img.publish(cv_bridge::CvImage(msg->header, "mono8", output).toImageMsg());
+			pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
 			// bridge_input is handled by a smart-pointer. No explicit delete needed.
 
             //droneTracking(Rect(Point(0,0), output.size()));
@@ -138,8 +138,8 @@ class Traite_image {
 			cur.copyTo(output);
 			cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
 			cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
-			GaussianBlur(prev_grey, prev_grey, Size(15,15), 3.0);
-			GaussianBlur(cur_grey, cur_grey, Size(15,15), 3.0);
+			GaussianBlur(prev_grey, prev_grey, Size(BLUR_SIZE,BLUR_SIZE), 3.0);
+			GaussianBlur(cur_grey, cur_grey, Size(BLUR_SIZE,BLUR_SIZE), 3.0);
 			//blur(prev_grey, prev_grey, Size(BLUR_SIZE, BLUR_SIZE));
 			//blur(cur_grey, cur_grey, Size(BLUR_SIZE, BLUR_SIZE));
 
@@ -149,15 +149,30 @@ class Traite_image {
 			// 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);
-			thres.copyTo(output);
+			threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
+			
+			//~ int dilation_size = 2;
+			//~ Mat element = getStructuringElement( MORPH_ELLIPSE,
+                                       //~ Size( 2*dilation_size + 1, 2*dilation_size+1 ),
+                                       //~ Point( dilation_size, dilation_size ) );
+			//~ // Apply the dilation operation
+			//~ Mat dilated_thres;
+			//~ dilate(thres, dilated_thres, element );
+			//~ 
+			//~ dilated_thres.copyTo(output);
+			
+			Mat closed_thres;
+			Mat structuringElement = getStructuringElement(MORPH_ELLIPSE, Size(40, 40));
+			morphologyEx( thres, closed_thres, MORPH_CLOSE, structuringElement );
+			
+			//closed_thres.copyTo(output);
 
 			//notice how we use the '&' operator for objectDetected and output. 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 output to be displayed in the main() function.
 			bool objectDetected = false;
 			Mat temp;
-			thres.copyTo(temp);
+			closed_thres.copyTo(temp);
 			//these two vectors needed for output of findContours
 			vector< vector<Point> > contours;
 			vector<Vec4i> hierarchy;
@@ -172,23 +187,27 @@ class Traite_image {
 			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));
+				//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));
+				for(int i=0; i<contours.size();i++) 
+				{
+					objectBoundingRectangle = boundingRect(contours[i]);
+					rectangle(output, objectBoundingRectangle, Scalar(0, 255, 0), 2);
+				}
 			}
 			//make some temp x and y variables so we dont have to type out so much
-			int x = objectBoundingRectangle.x;
-			int y = objectBoundingRectangle.y;
-			int width = objectBoundingRectangle.width;
-			int height = objectBoundingRectangle.height;
+			//~ int x = objectBoundingRectangle.x;
+			//~ int y = objectBoundingRectangle.y;
+			//~ int width = objectBoundingRectangle.width;
+			//~ int height = objectBoundingRectangle.height;
 
 			//draw a rectangle around the object
-			rectangle(output, Point(x,y), Point(x+width, y+height), Scalar(0, 255, 0), 2);
+			//rectangle(output, Point(x,y), Point(x+width, y+height), Scalar(0, 255, 0), 2);
 
 			//write the position of the object to the screen
-			putText(output,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2);
+			//putText(output,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2);
 		}
 
 		inline bool isFlowCorrect(Point2f u)