lhark/papillon
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Improve tracking

Browse source
This commit is contained in:
lhark 2016-06-14 16:17:48 +02:00
parent 28435564d6
commit 8fe8b8a1ca
2 changed files with 32 additions and 19 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

5
commandes Normal file
View file

@ -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

46
src/papillon.cpp
View file

@ -12,9 +12,10 @@ using namespace std;
class Traite_image { class Traite_image {
public: public:
const static int SENSITIVITY_VALUE = 30; const static int SENSITIVITY_VALUE = 40;
const static int BLUR_Size = 15; const static int BLUR_Size = 9;
const static int CLOSE_SIZE = 50; const static int CLOSE_SIZE = 20;
const static int ERODE_SIZE = 2;
cv::Mat prev; cv::Mat prev;
@ -29,14 +30,16 @@ class Traite_image {
image_transport::ImageTransport it; image_transport::ImageTransport it;
image_transport::Publisher pub_img; image_transport::Publisher pub_img;
image_transport::Publisher pub_thres;
ros::Publisher pub_cmd; ros::Publisher pub_cmd;
image_transport::Subscriber sub; image_transport::Subscriber sub;
Traite_image() : n("~"),it(n) { Traite_image() : n("~"),it(n) {
pub_img = it.advertise("/image_out", 1); pub_img = it.advertise("/image_out", 1);
pub_thres = it.advertise("/thres_out", 1);
pub_cmd = n.advertise<papillon::BoundingBox>("/bbox", 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::Rect myROI(crop_x, crop_y, crop_w, crop_h);
cv::Mat next_stab_cropped = next_stab(myROI); cv::Mat next_stab_cropped = next_stab(myROI);
cv::Mat prev_cropped = prev(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_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. // bridge_input is handled by a smart-pointer. No explicit delete needed.
//droneTracking(Rect(Point(0,0), output.size())); //droneTracking(Rect(Point(0,0), output.size()));
@ -133,7 +138,7 @@ class Traite_image {
cur2.copyTo(output); 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; cv::Mat cur_grey, prev_grey;
cur.copyTo(output); cur.copyTo(output);
cv::cvtColor(prev, prev_grey, cv::COLOR_BGR2GRAY); cv::cvtColor(prev, prev_grey, cv::COLOR_BGR2GRAY);
@ -146,25 +151,28 @@ class Traite_image {
// Subtract the 2 last frames and threshold them // Subtract the 2 last frames and threshold them
cv::Mat thres; cv::Mat thres;
cv::absdiff(prev_grey,cur_grey,thres); cv::absdiff(prev_grey,cur_grey,thres);
thres = 3 * thres;
// threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY); // threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
// // Blur to eliminate noise // // Blur to eliminate noise
// blur(thres, thres, cv::Size(BLUR_Size, BLUR_Size)); // 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); 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 closed_thres;
cv::Mat structuringElement = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(CLOSE_SIZE, CLOSE_SIZE)); cv::Mat structuringElement = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(CLOSE_SIZE, CLOSE_SIZE));
cv::morphologyEx( thres, closed_thres, cv::MORPH_CLOSE, structuringElement ); cv::morphologyEx( thres, closed_thres, cv::MORPH_CLOSE, structuringElement );
// dilated_thres.copyTo(output);
//closed_thres.copyTo(output); //closed_thres.copyTo(output);
//notice how we use the '&' operator for objectDetected and output. This is because we wish //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)); //largestContourVec.push_back(contours.at(contours.size()-1));
//make a bounding rectangle around the largest contour then find its centroid //make a bounding rectangle around the largest contour then find its centroid
//this will be the object's final esticv::Mated position. //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]); //~ objectBoundingRectangle = cv::boundingRect(contours[i]);
//~ cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2); //~ cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);