diff --git a/src/papillon.cpp b/src/papillon.cpp index 927af9a..da8ac51 100644 --- a/src/papillon.cpp +++ b/src/papillon.cpp @@ -20,7 +20,7 @@ class Traite_image { Mat prev; Mat last_T; bool first = true; - int resize_f = 4; + int resize_f = 1; int theObject[2] = {0,0}; Rect objectBoundingRectangle = Rect(0,0,0,0); @@ -63,16 +63,10 @@ class Traite_image { ROS_INFO("first done"); } - stabiliseImg(prev, next, output); + Mat next_stab; + stabiliseImg(prev, next, next_stab); + searchForMovement(prev, next_stab, output); - // Subtract the 2 last frames and threshold them - //Mat thres; - //absdiff(prev,next,thres); - //threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY); - // Blur to eliminate noise - //blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE)); - //threshold(thres, output, SENSITIVITY_VALUE, 255, THRESH_BINARY); - //searchForMovement(thres, output); pub.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg()); // bridge_input is handled by a smart-pointer. No explicit delete needed. @@ -92,12 +86,9 @@ class Traite_image { } void stabiliseImg(Mat prev, Mat cur, Mat &output){ - Mat prev_grey, cur_grey; + Mat cur_grey, prev_grey; cvtColor(cur, cur_grey, COLOR_BGR2GRAY); cvtColor(prev, prev_grey, COLOR_BGR2GRAY); - //Point2f srcTri[3]; - //Point2f dstTri[3]; - Mat warp_mat( 2, 3, CV_32FC1 ); // vector from prev to cur vector prev_corner, cur_corner; @@ -116,88 +107,40 @@ class Traite_image { } } - // translation + rotation only - // Mat T(2, 3, CV_32FC1); - Mat T = estimateRigidTransform(prev_corner2, cur_corner2, false); // false = rigid transform, no scaling/shearing + Mat T = estimateRigidTransform(prev_corner2, cur_corner2, true); // false = rigid transform, no scaling/shearing - ROS_INFO("coucou1"); - // in rare cases no transform is found. We'll just use the last known good transform. - /*if(T.data == NULL) { - last_T = T.clone(); - } + if(T.data == NULL) { + last_T.copyTo(T); + } + T.copyTo(last_T); - T = last_T.clone();*/ - ROS_INFO("coucou2"); - - // decompose T - double dx = T.at(0,2); - double dy = T.at(1,2); - double da = atan2(T.at(1,0), T.at(0,0)); - // cv::Mat H = cv::Mat(3,3,T.type()); - // H.at(0,0) = T.at(0,0); - // H.at(0,1) = T.at(0,1); - // H.at(0,2) = T.at(0,2); + Mat cur2; - // H.at(1,0) = T.at(1,0); - // H.at(1,1) = T.at(1,1); - // H.at(1,2) = T.at(1,2); + warpAffine(cur, cur2, T, cur.size(),INTER_NEAREST|WARP_INVERSE_MAP); - // H.at(2,0) = 0.0; - // H.at(2,1) = 0.0; - // H.at(2,2) = 1.0; - - Mat T1(2, 3, CV_32FC1); - T1.at(0,0) = cos(da); - T1.at(0,1) = -sin(da); - T1.at(1,0) = sin(da); - T1.at(1,1) = cos(da); - - T1.at(0,2) = dx; - T1.at(1,2) = dy; - - Mat cur2; - - warpAffine(cur, cur2, T1, cur.size()); - - - int vert_border = HORIZONTAL_BORDER_CROP * prev.rows / prev.cols; // get the aspect ratio correct - - cur2 = cur2(Range(vert_border, cur2.rows-vert_border), Range(HORIZONTAL_BORDER_CROP, cur2.cols-HORIZONTAL_BORDER_CROP)); - - // Resize cur2 back to cur size, for better side by side comparison - resize(cur2, cur2, cur.size()); - - output = cur2.clone(); - - /* - - /// Set your 3 points to calculate the Affine Transform - srcTri[0] = Point2f( 0,0 ); - srcTri[1] = Point2f( prev.cols, 0 ); - srcTri[2] = Point2f( 0, prev.rows ); - - dstTri[0] = Point2f( prev.cols / 2, prev.rows / 2 ); - dstTri[1] = Point2f( prev.cols * 3 / 2, prev.rows / 2); - dstTri[2] = Point2f( prev.cols / 2, prev.rows * 3 / 2 ); - - /// Get the Affine Transform - warp_mat = getAffineTransform( srcTri, dstTri ); - warpAffine(prev,output,T,cv::Size(prev.cols+cur.cols,prev.rows+cur.rows)); - warpAffine(output,output,warp_mat,cv::Size(prev.cols+cur.cols,prev.rows+cur.rows)); - warpAffine(cur,cur,warp_mat,cv::Size(prev.cols+cur.cols,prev.rows+cur.rows)); - //Mat half(output, cv::Rect(0, 0,cur.cols,cur.rows)); - cur.copyTo(output, cur); - * - */ + cur2.copyTo(output); } - void searchForMovement(Mat thresholdImage, Mat &cameraFeed){ - //notice how we use the '&' operator for objectDetected and cameraFeed. This is because we wish + 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, SENSITIVITY_VALUE, 255, THRESH_BINARY); + // Blur to eliminate noise + blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE)); + threshold(thres, thres, SENSITIVITY_VALUE, 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 cameraFeed to be displayed in the main() function. + //eg. we draw to the output to be displayed in the main() function. bool objectDetected = false; Mat temp; - thresholdImage.copyTo(temp); + thres.copyTo(temp); //these two vectors needed for output of findContours vector< vector > contours; vector hierarchy; @@ -228,14 +171,14 @@ class Traite_image { int y = theObject[1]; //draw some crosshairs around the object - circle(cameraFeed,Point(x,y),20,Scalar(0,255,0),2); - line(cameraFeed,Point(x,y),Point(x,y-25),Scalar(0,255,0),2); - line(cameraFeed,Point(x,y),Point(x,y+25),Scalar(0,255,0),2); - line(cameraFeed,Point(x,y),Point(x-25,y),Scalar(0,255,0),2); - line(cameraFeed,Point(x,y),Point(x+25,y),Scalar(0,255,0),2); + circle(output,Point(x,y),20,Scalar(0,255,0),2); + line(output,Point(x,y),Point(x,y-25),Scalar(0,255,0),2); + line(output,Point(x,y),Point(x,y+25),Scalar(0,255,0),2); + line(output,Point(x,y),Point(x-25,y),Scalar(0,255,0),2); + line(output,Point(x,y),Point(x+25,y),Scalar(0,255,0),2); //write the position of the object to the screen - putText(cameraFeed,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2); + putText(output,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2); } inline bool isFlowCorrect(Point2f u)