From ce831fb167647d6c28e4ff781493a880650d1da2 Mon Sep 17 00:00:00 2001 From: samilyjcc Date: Wed, 1 Jun 2016 17:09:28 +0200 Subject: [PATCH] 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) {