diff --git a/src/papillon.cpp b/src/papillon.cpp index 626d510..7604a15 100644 --- a/src/papillon.cpp +++ b/src/papillon.cpp @@ -17,18 +17,24 @@ class Traite_image { const static int THRESHOLD_DETECT_SENSITIVITY = 10; const static int BLUR_SIZE = 5; const static int THRESHOLD_MOV = 5; - const static int MOVEMENT_THRES = 0.1; + constexpr static float MOVEMENT_THRES = 0.1; + + constexpr static float FLOW_MIN_QUAL = 0.01; + const static int FLOW_MIN_DIST = 20; Mat prev; - Mat last_T; + + // Stabilisation transformation matrices + Mat T, last_T; + bool first = true; // Features vectors vector prev_ftr, cur_ftr; // Downsize factor - int resize_f = 2; + int resize_f = 1; int theObject[2] = {0,0}; Rect objectBoundingRectangle = Rect(0,0,0,0); @@ -75,10 +81,10 @@ class Traite_image { Mat next_stab; stabiliseImg(prev, next, next_stab); - 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); - trackingOptFlow(prev_cropped, next_stab_cropped, output); + trackingOptFlow(prev, next_stab, next_stab); + Mat next_stab2; + stabiliseImg(prev, next, next_stab2); + trackingOptFlow(prev, next_stab2, output); //searchForMovementOptFlow(prev_cropped, next_stab_cropped, output); @@ -112,7 +118,7 @@ class Traite_image { vector status; vector err; - goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30); + goodFeaturesToTrack(prev_grey, prev_corner, 200, FLOW_MIN_QUAL, FLOW_MIN_DIST); calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err); // weed out bad matches @@ -125,12 +131,12 @@ class Traite_image { } } - Mat T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing + T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing - if(T.data == NULL) { + if(T.data == NULL) last_T.copyTo(T); - } - T.copyTo(last_T); + else + T.copyTo(last_T); Mat cur2; @@ -139,115 +145,6 @@ class Traite_image { cur2.copyTo(output); } - void searchForMovement(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); - - // Subtract the 2 last frames and threshold them - Mat thres; - absdiff(prev_grey,cur_grey,thres); - threshold(thres, thres, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY); - // Blur to eliminate noise - blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE)); - 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. - //eg. we draw to the output to be displayed in the main() function. - bool objectDetected = false; - Mat temp; - thres.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 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, 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, THRESHOLD_DETECT_SENSITIVITY, 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 warpPoints(vector p, vector &p_warp, Mat T, bool invert=false) { if(invert) invertAffineTransform(T, T); @@ -266,17 +163,27 @@ class Traite_image { } void trackingOptFlow(Mat prev, Mat cur, Mat &output) { - vector curc_stab; + cur.copyTo(output); + vector cur_ftr_stab; + Mat T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing - ROS_INFO("ready to warp"); - warpPoints(cur_ftr, curc_stab, T, true); - ROS_INFO("warped"); + if(T.data == NULL) + last_T.copyTo(T); + else + T.copyTo(last_T); + + warpPoints(cur_ftr, cur_ftr_stab, T, true); vector objects; for(size_t i=0; i < prev_ftr.size(); ++i) { - float flow_norm = norm(prev_ftr[i] - cur_ftr[i]) / prev.size().height; - if(flow_norm > MOVEMENT_THRES) - objects.push_back(cur_ftr[i]); + float flow_norm = norm(prev_ftr[i] - cur_ftr_stab[i]) / prev.size().height; + line(output, prev_ftr[i], cur_ftr[i], Scalar(200,0,0),1); + line(output, prev_ftr[i], cur_ftr_stab[i], Scalar(0,200,0),1); + if(flow_norm > MOVEMENT_THRES) { + objects.push_back(cur_ftr_stab[i]); + prev_ftr.erase(prev_ftr.begin() + i); + cur_ftr.erase(cur_ftr.begin() + i); + } } for(size_t i=0; i < objects.size(); ++i) {