From 8fe8b8a1cac9ef71012562660631488d127f4ade Mon Sep 17 00:00:00 2001 From: lhark Date: Tue, 14 Jun 2016 16:17:48 +0200 Subject: [PATCH] Improve tracking --- commandes | 5 +++++ src/papillon.cpp | 46 +++++++++++++++++++++++++++------------------- 2 files changed, 32 insertions(+), 19 deletions(-) create mode 100644 commandes diff --git a/commandes b/commandes new file mode 100644 index 0000000..f6338f2 --- /dev/null +++ b/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 diff --git a/src/papillon.cpp b/src/papillon.cpp index 360ce71..59f160b 100644 --- a/src/papillon.cpp +++ b/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("/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