diff --git a/src/papillon.cpp b/src/papillon.cpp
index 5dc61bf..3d35ba1 100644
--- a/src/papillon.cpp
+++ b/src/papillon.cpp
@@ -120,6 +120,9 @@ class Traite_image {
 			Mat T(2, 3, CV_32FC1);
 			T = estimateRigidTransform(prev_corner2, cur_corner2, false); // false = rigid transform, no scaling/shearing
 
+			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));
 			// 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);
@@ -133,6 +136,13 @@ 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);
+
+			T.at<double>(0,2) = dx;
+			T.at<double>(1,2) = dy;
 
 			// in rare cases no transform is found. We'll just use the last known good transform.
 			if(T.data == NULL) {