From 8bd67f32036112be6a64ba95320ef6d7101d3abd Mon Sep 17 00:00:00 2001
From: samilyjcc <lejcdu84@hotmail.fr>
Date: Mon, 23 May 2016 15:58:52 +0200
Subject: [PATCH] copying code

---
 src/papillon.cpp | 57 +++++++++++++++++++++++++++++++++---------------
 1 file changed, 40 insertions(+), 17 deletions(-)

diff --git a/src/papillon.cpp b/src/papillon.cpp
index 3d35ba1..927af9a 100644
--- a/src/papillon.cpp
+++ b/src/papillon.cpp
@@ -14,6 +14,8 @@ class Traite_image {
 	public:
 		const static int SENSITIVITY_VALUE = 30;
 		const static int BLUR_SIZE = 10;
+        const int HORIZONTAL_BORDER_CROP = 20; // In pixels. Crops the border to reduce the black borders from stabilisation being too noticeable.
+
 
 		Mat prev;
 		Mat last_T;
@@ -52,9 +54,7 @@ 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_grey, CV_BGR2GRAY);
 			Mat output;// = input.clone(); //   (input.rows, input.cols, CV_32FC2);
 			//ROS_INFO("got input");
 			if (first) {
@@ -95,8 +95,8 @@ class Traite_image {
 			Mat prev_grey, cur_grey;
 			cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
 			cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
-			Point2f srcTri[3];
-			Point2f dstTri[3];
+			//Point2f srcTri[3];
+			//Point2f dstTri[3];
 			Mat warp_mat( 2, 3, CV_32FC1 );
 
 			// vector from prev to cur
@@ -117,9 +117,19 @@ class Traite_image {
 			}
 
 			// translation + rotation only
-			Mat T(2, 3, CV_32FC1);
-			T = estimateRigidTransform(prev_corner2, cur_corner2, false); // false = rigid transform, no scaling/shearing
+			// Mat T(2, 3, CV_32FC1);
+			Mat T = estimateRigidTransform(prev_corner2, cur_corner2, false); // 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();
+            }
+
+            T = last_T.clone();*/
+            ROS_INFO("coucou2");
+            
+            // decompose T
 			double dx = T.at<double>(0,2);
 			double dy = T.at<double>(1,2);
 			double da = atan2(T.at<double>(1,0), T.at<double>(0,0));
@@ -136,19 +146,30 @@ class Traite_image {
 			// H.at<double>(2,1) = 0.0;
 			// H.at<double>(2,2) = 1.0;
 
-			T.at<double>(0,0) = cos(da);
-			T.at<double>(0,1) = -sin(da);
-			T.at<double>(1,0) = sin(da);
-			T.at<double>(1,1) = cos(da);
+            Mat T1(2, 3, CV_32FC1);
+			T1.at<double>(0,0) = cos(da);
+			T1.at<double>(0,1) = -sin(da);
+			T1.at<double>(1,0) = sin(da);
+			T1.at<double>(1,1) = cos(da);
 
-			T.at<double>(0,2) = dx;
-			T.at<double>(1,2) = dy;
+			T1.at<double>(0,2) = dx;
+			T1.at<double>(1,2) = dy;
+            
+            Mat cur2;
 
-			// 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);
+            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 );
@@ -166,6 +187,8 @@ class Traite_image {
 			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){