From ce831fb167647d6c28e4ff781493a880650d1da2 Mon Sep 17 00:00:00 2001 From: samilyjcc Date: Wed, 1 Jun 2016 17:09:28 +0200 Subject: [PATCH 1/2] changed detection to opt flow --- src/papillon.cpp | 68 +++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 65 insertions(+), 3 deletions(-) diff --git a/src/papillon.cpp b/src/papillon.cpp index 34dce88..197ca2e 100644 --- a/src/papillon.cpp +++ b/src/papillon.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include @@ -13,8 +14,8 @@ using namespace std; class Traite_image { public: - const static int SENSITIVITY_VALUE = 30; - const static int BLUR_SIZE = 10; + const static int SENSITIVITY_VALUE = 10; + const static int BLUR_SIZE = 5; Mat prev; @@ -70,7 +71,7 @@ class Traite_image { Rect myROI(next_stab.size().width/8, next_stab.size().height/8, next_stab.size().width*3/4, next_stab.size().height*3/4); Mat next_stab_cropped = next_stab(myROI); Mat prev_cropped = prev(myROI); - searchForMovement(prev_cropped, next_stab_cropped, output); + searchForMovementOptFlow(prev_cropped, next_stab_cropped, output); pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg()); @@ -180,6 +181,67 @@ class Traite_image { //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); } + + void searchForMovementOptFlow(Mat prev, Mat cur, Mat &output){ + Mat cur_grey, prev_grey; + cur.copyTo(output); + cvtColor(prev, prev_grey, COLOR_BGR2GRAY); + cvtColor(cur, cur_grey, COLOR_BGR2GRAY); + + Mat flow; + calcOpticalFlowFarneback(prev_grey, cur_grey, flow, 0.5, 3, 15, 3, 5, 1.2, 0); + vector flow_coord(2); + Mat flow_norm, angle; + split(flow, flow_coord); + cartToPolar(flow_coord[0], flow_coord[1], flow_norm, angle); + + //threshold(flow_norm, flow_norm, SENSITIVITY_VALUE, 255, THRESH_BINARY); + // Blur to eliminate noise + blur(flow_norm, flow_norm, Size(BLUR_SIZE, BLUR_SIZE)); + threshold(flow_norm, flow_norm, SENSITIVITY_VALUE, 255, THRESH_BINARY); + flow_norm.convertTo(flow_norm, CV_8U); + + bool objectDetected = false; + Mat temp; + flow_norm.copyTo(temp); + //these two vectors needed for output of findContours + vector< vector > contours; + vector hierarchy; + //find contours of filtered image using openCV findContours function + //findContours(temp,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE );// retrieves all contours + findContours(temp,contours,hierarchy,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE );// retrieves external contours + + //if contours vector is not empty, we have found some objects + if(contours.size()>0)objectDetected=true; + else objectDetected = false; + + 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 > 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)); + } + //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; + + //draw a rectangle around the object + 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); + + } + + void get_norm(Mat coord, Mat &norm) + { + //for(i=0; i< + } inline bool isFlowCorrect(Point2f u) { From e4ac1b70afc94591994fb76b8e8c558299ad73b9 Mon Sep 17 00:00:00 2001 From: samilyjcc Date: Wed, 1 Jun 2016 17:45:32 +0200 Subject: [PATCH 2/2] add threshold to movement --- src/papillon.cpp | 22 +++++++++------------- 1 file changed, 9 insertions(+), 13 deletions(-) diff --git a/src/papillon.cpp b/src/papillon.cpp index 197ca2e..b6ad74c 100644 --- a/src/papillon.cpp +++ b/src/papillon.cpp @@ -14,14 +14,15 @@ using namespace std; class Traite_image { public: - const static int SENSITIVITY_VALUE = 10; + const static int THRESHOLD_DETECT_SENSITIVITY = 10; const static int BLUR_SIZE = 5; + const static int THRESHOLD_MOV = 5; Mat prev; Mat last_T; bool first = true; - int resize_f = 1; + int resize_f = 2; int theObject[2] = {0,0}; Rect objectBoundingRectangle = Rect(0,0,0,0); @@ -138,10 +139,10 @@ class Traite_image { // Subtract the 2 last frames and threshold them Mat thres; absdiff(prev_grey,cur_grey,thres); - threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY); + threshold(thres, thres, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY); // Blur to eliminate noise blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE)); - threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY); + threshold(thres, thres, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY); //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. @@ -195,10 +196,10 @@ class Traite_image { split(flow, flow_coord); cartToPolar(flow_coord[0], flow_coord[1], flow_norm, angle); - //threshold(flow_norm, flow_norm, SENSITIVITY_VALUE, 255, THRESH_BINARY); + //threshold(flow_norm, flow_norm, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY); // Blur to eliminate noise blur(flow_norm, flow_norm, Size(BLUR_SIZE, BLUR_SIZE)); - threshold(flow_norm, flow_norm, SENSITIVITY_VALUE, 255, THRESH_BINARY); + threshold(flow_norm, flow_norm, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY); flow_norm.convertTo(flow_norm, CV_8U); bool objectDetected = false; @@ -236,11 +237,6 @@ class Traite_image { //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); - } - - void get_norm(Mat coord, Mat &norm) - { - //for(i=0; i< } inline bool isFlowCorrect(Point2f u) @@ -256,11 +252,11 @@ class Traite_image { geometry_msgs::Twist twist = geometry_msgs::Twist(); - if(centre_rect.x < centre_image.x) + if(centre_rect.x < centre_image.x-THRESHOLD_MOV) { twist.angular.z = 0.2; } - else if(centre_rect.x > centre_image.x) + else if(centre_rect.x > centre_image.x+THRESHOLD_MOV) { twist.angular.z = -0.2; }