From 3dbe6d192595e7adf0648bfe02da5944faf3bf05 Mon Sep 17 00:00:00 2001 From: lhark Date: Wed, 18 May 2016 00:46:44 +0200 Subject: [PATCH] Add background movement compensation --- src/papillon.cpp | 89 +++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 80 insertions(+), 9 deletions(-) diff --git a/src/papillon.cpp b/src/papillon.cpp index 7e1e71a..5dc61bf 100644 --- a/src/papillon.cpp +++ b/src/papillon.cpp @@ -16,8 +16,9 @@ class Traite_image { const static int BLUR_SIZE = 10; Mat prev; + Mat last_T; bool first = true; - int resize_f = 1; + int resize_f = 4; int theObject[2] = {0,0}; Rect objectBoundingRectangle = Rect(0,0,0,0); @@ -51,9 +52,10 @@ class Traite_image { //Mat& input = const_cast(bridge_input->image); const Mat& input = bridge_input->image; Mat next; + Mat next_grey; resize(input, next, Size(input.size().width/resize_f, input.size().height/resize_f)); - cvtColor(next, next, CV_BGR2GRAY); - Mat output = input.clone(); // (input.rows, input.cols, CV_32FC2); + cvtColor(next, next_grey, CV_BGR2GRAY); + Mat output;// = input.clone(); // (input.rows, input.cols, CV_32FC2); //ROS_INFO("got input"); if (first) { prev = next.clone(); @@ -61,14 +63,16 @@ class Traite_image { ROS_INFO("first done"); } + stabiliseImg(prev, next, output); + // Subtract the 2 last frames and threshold them - Mat thres; - absdiff(prev,next,thres); - threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY); + //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, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY); - searchForMovement(thres, output); + //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. @@ -87,6 +91,73 @@ class Traite_image { return ss.str(); } + void stabiliseImg(Mat prev, Mat cur, Mat &output){ + Mat prev_grey, cur_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; + vector prev_corner2, cur_corner2; + vector status; + vector err; + + goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30); + calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err); + + // weed out bad matches + for(size_t i=0; i < status.size(); i++) { + if(status[i]) { + prev_corner2.push_back(prev_corner[i]); + cur_corner2.push_back(cur_corner[i]); + } + } + + // translation + rotation only + Mat T(2, 3, CV_32FC1); + T = estimateRigidTransform(prev_corner2, cur_corner2, false); // false = rigid transform, no scaling/shearing + + // 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); + + // H.at(1,0) = T.at(1,0); + // H.at(1,1) = T.at(1,1); + // H.at(1,2) = T.at(1,2); + + // H.at(2,0) = 0.0; + // H.at(2,1) = 0.0; + // H.at(2,2) = 1.0; + + + // in rare cases no transform is found. We'll just use the last known good transform. + if(T.data == NULL) { + last_T.copyTo(T); + } + T.copyTo(last_T); + + /// 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); + } + void searchForMovement(Mat thresholdImage, Mat &cameraFeed){ //notice how we use the '&' operator for objectDetected and cameraFeed. This is because we wish //to take the values passed into the function and manipulate them, rather than just working with a copy.