From 3dbe6d192595e7adf0648bfe02da5944faf3bf05 Mon Sep 17 00:00:00 2001
From: lhark <lhark@hotmail.fr>
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<Mat&>(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 <Point2f> prev_corner, cur_corner;
+			vector <Point2f> prev_corner2, cur_corner2;
+			vector <uchar> status;
+			vector <float> 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<double>(0,0) = T.at<double>(0,0);
+			// H.at<double>(0,1) = T.at<double>(0,1);
+			// H.at<double>(0,2) = T.at<double>(0,2);
+
+			// H.at<double>(1,0) = T.at<double>(1,0);
+			// H.at<double>(1,1) = T.at<double>(1,1);
+			// H.at<double>(1,2) = T.at<double>(1,2);
+
+			// H.at<double>(2,0) = 0.0;
+			// H.at<double>(2,1) = 0.0;
+			// H.at<double>(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.