Improve tracking

commandes

@@ -0,0 +1,5 @@
+rosrun demo_teleop key_teleop.py --persist /cmd_vel:=/bebop/cmd_vel
+rostopic pub --once /bebop/land std_msgs/Empty
+rostopic pub --once /bebop/takeoff std_msgs/Empty
+catkin_make --pkg=papillon
+roslaunch bebop_driver bebop_node.launch

src/papillon.cpp

@@ -12,9 +12,10 @@ using namespace std;
 
 class Traite_image {
 	public:
-		const static int SENSITIVITY_VALUE = 30;
-		const static int BLUR_Size = 15;
-		const static int CLOSE_SIZE = 50;
+		const static int SENSITIVITY_VALUE = 40;
+		const static int BLUR_Size = 9;
+		const static int CLOSE_SIZE = 20;
+		const static int ERODE_SIZE = 2;
 
 
 		cv::Mat prev;
@@ -29,14 +30,16 @@ class Traite_image {
 
 		image_transport::ImageTransport it;
 		image_transport::Publisher      pub_img;
+		image_transport::Publisher      pub_thres;
         ros::Publisher					pub_cmd;
 		image_transport::Subscriber     sub;
 
 
 		Traite_image() : n("~"),it(n) {
 			pub_img = it.advertise("/image_out", 1);
+			pub_thres = it.advertise("/thres_out", 1);
 			pub_cmd = n.advertise<papillon::BoundingBox>("/bbox", 1);
-			sub = it.subscribe("/usb_cam/image_raw", 1, [this](const sensor_msgs::ImageConstPtr& img) -> void { this->on_image(img);},ros::VoidPtr(),image_transport::TransportHints("compressed"));
+			sub = it.subscribe("/bebop/image_raw", 1, [this](const sensor_msgs::ImageConstPtr& img) -> void { this->on_image(img);},ros::VoidPtr(),image_transport::TransportHints("compressed"));
 		}
 
 
@@ -75,10 +78,12 @@ class Traite_image {
 			cv::Rect myROI(crop_x, crop_y, crop_w, crop_h);
 			cv::Mat next_stab_cropped = next_stab(myROI);
 			cv::Mat prev_cropped = prev(myROI);
-			searchForMovement(prev_cropped, next_stab_cropped, output);
+			cv::Mat closed_thres;
+			searchForMovement(prev_cropped, next_stab_cropped, output, closed_thres);
 
 
 			pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
+			pub_thres.publish(cv_bridge::CvImage(msg->header, "mono8", closed_thres).toImageMsg());
 			// bridge_input is handled by a smart-pointer. No explicit delete needed.
 
             //droneTracking(Rect(Point(0,0), output.size()));
@@ -133,7 +138,7 @@ class Traite_image {
 			cur2.copyTo(output);
 		}
 
-		void searchForMovement(cv::Mat prev, cv::Mat cur, cv::Mat &output){
+		void searchForMovement(cv::Mat prev, cv::Mat cur, cv::Mat &output, cv::Mat &out2){
 			cv::Mat cur_grey, prev_grey;
 			cur.copyTo(output);
 			cv::cvtColor(prev, prev_grey, cv::COLOR_BGR2GRAY);
@@ -146,25 +151,28 @@ 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));
+
+			cv::Mat element = getStructuringElement( cv::MORPH_ELLIPSE,
+                                   cv::Size( 2*ERODE_SIZE + 1, 2*ERODE_SIZE+1 ),
+                                   cv::Point( ERODE_SIZE, ERODE_SIZE ) );
+			// Apply the dilation operation
+			cv::erode(thres, thres, element );
+
+
+			thres.copyTo(out2);
+
 			cv::threshold(thres, thres, SENSITIVITY_VALUE, 255, cv::THRESH_BINARY);
-			
-			//~ int dilation_Size = 2;
-			//~ cv::Mat element = getStructuringElement( MORPH_ELLIPSE,
-                                       //~ cv::Size( 2*dilation_Size + 1, 2*dilation_Size+1 ),
-                                       //~ Point( dilation_Size, dilation_Size ) );
-			//~ // Apply the dilation operation
-			//~ cv::Mat dilated_thres;
-			//~ dilate(thres, dilated_thres, element );
-			//~ 
-			//~ dilated_thres.copyTo(output);
-			
+
 			cv::Mat closed_thres;
 			cv::Mat structuringElement = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(CLOSE_SIZE, CLOSE_SIZE));
 			cv::morphologyEx( thres, closed_thres, cv::MORPH_CLOSE, structuringElement );
-			
+			// dilated_thres.copyTo(output);
+
+
 			//closed_thres.copyTo(output);
 
 			//notice how we use the '&' operator for objectDetected and output. This is because we wish
@@ -191,7 +199,7 @@ 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++) 
+				//~ for(int i=0; i<contours.size();i++)
 				//~ {
 					//~ objectBoundingRectangle = cv::boundingRect(contours[i]);
 					//~ cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);