From 284c115b1c990d86c645a827b43a865bd40890cb Mon Sep 17 00:00:00 2001
From: lhark <lhark@hotmail.fr>
Date: Wed, 15 Jun 2016 17:29:51 +0200
Subject: [PATCH] Tracking tweaks

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

diff --git a/src/papillon.cpp b/src/papillon.cpp
index 054a856..0ab4486 100644
--- a/src/papillon.cpp
+++ b/src/papillon.cpp
@@ -12,11 +12,11 @@ using namespace std;
 
 class Traite_image {
 	public:
-		const static int SENSITIVITY_VALUE = 40;
+		const static int SENSITIVITY_VALUE = 35;
 		const static int BLUR_Size = 9;
 		const static int CLOSE_SIZE = 20;
-		const static int ERODE_SIZE = 2;
-		const static int NB_FRAME_DROP = 2;
+		const static int ERODE_SIZE = 5;
+		const static int NB_FRAME_DROP = 3;
 
 
 		vector<cv::Mat> prevs;
@@ -25,7 +25,6 @@ class Traite_image {
 		int resize_f = 1;
 
 		int theObject[2] = {0,0};
-		cv::Rect objectBoundingRectangle = cv::Rect(0,0,0,0);
 
 		ros::NodeHandle n;
 
@@ -62,7 +61,6 @@ class Traite_image {
 			cv::Mat next;
 			resize(input, next, cv::Size(input.size().width/resize_f, input.size().height/resize_f));
 			cv::Mat output;// = input.clone(); //   (input.rows, input.cols, CV_32FC2);
-			//ROS_INFO("got input");
 			if (first) {
 				for (int i = 0; i < NB_FRAME_DROP; ++i) {
 					prevs.push_back(next.clone());
@@ -91,8 +89,6 @@ class Traite_image {
 
             //droneTracking(Rect(Point(0,0), output.size()));
 
-			//ROS_INFO("pub");
-
 			prevs.pop_back();
 			prevs.insert(prevs.begin(), next.clone());
 		}
@@ -155,7 +151,6 @@ class Traite_image {
 			// Subtract the 2 last frames and threshold them
 			cv::Mat thres;
 			cv::absdiff(prev_grey,cur_grey,thres);
-			thres = 3 * thres;
 			// threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
 			// // Blur to eliminate noise
 			// blur(thres, thres, cv::Size(BLUR_Size, BLUR_Size));
@@ -166,6 +161,7 @@ class Traite_image {
 			// Apply the dilation operation
 			cv::erode(thres, thres, element );
 
+			thres.copyTo(out2);
 
 
 			cv::threshold(thres, thres, SENSITIVITY_VALUE, 255, cv::THRESH_BINARY);
@@ -175,7 +171,6 @@ class Traite_image {
 			cv::morphologyEx( thres, closed_thres, cv::MORPH_CLOSE, structuringElement );
 			// dilated_thres.copyTo(output);
 
-			closed_thres.copyTo(out2);
 
 			//closed_thres.copyTo(output);
 
@@ -203,13 +198,20 @@ class Traite_image {
 				//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 esticv::Mated position.
-				//~ for(int i=0; i<contours.size();i++)
-				//~ {
-					//~ objectBoundingRectangle = cv::boundingRect(contours[i]);
-					//~ cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);
-				//~ }
-				objectBoundingRectangle = cv::boundingRect(contours.at(contours.size()-1));
-				cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);
+				cv::Rect objectBoundingRectangle;
+				for(int i=0; i<contours.size();i++)
+				{
+					objectBoundingRectangle = cv::boundingRect(contours[i]);
+					cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);
+				}
+				cv::Rect objBRect = cv::boundingRect(contours.at(contours.size()-1));
+				//cv::rectangle(output, objBRect, cv::Scalar(0, 255, 0), 2);
+				papillon::BoundingBox bbox = papillon::BoundingBox();
+				bbox.x = objBRect.x / (float)cur.size().width;
+				bbox.y = objBRect.y / (float)cur.size().height;
+				bbox.width = objBRect.width / (float)cur.size().width;
+				bbox.height = objBRect.height / (float)cur.size().height;
+				pub_cmd.publish(bbox);
 			}
 			//make some temp x and y variables so we dont have to type out so much
 			//~ int x = objectBoundingRectangle.x;