Fix recursive stabilisation

src/papillon.cpp

@@ -17,7 +17,7 @@ class Traite_image {
 		const static int THRESHOLD_DETECT_SENSITIVITY = 10;
 		const static int BLUR_SIZE = 5;
 		const static int THRESHOLD_MOV = 5;
-		constexpr static float MOVEMENT_THRES = 0.1;
+		constexpr static float MOVEMENT_THRES = 0.05;
 
 		constexpr static float FLOW_MIN_QUAL = 0.01;
 		const static int FLOW_MIN_DIST = 20;
@@ -80,10 +80,12 @@ class Traite_image {
 			}
 
 			Mat next_stab;
-			stabiliseImg(prev, next, next_stab);
+			trackFeatures(prev, next);
+			stabiliseImg(next, next_stab);
 			trackingOptFlow(prev, next_stab, next_stab);
 			Mat next_stab2;
-			stabiliseImg(prev, next, next_stab2);
+			trackFeatures(prev, next);
+			stabiliseImg(next, next_stab2);
 			trackingOptFlow(prev, next_stab2, output);
 			//searchForMovementOptFlow(prev_cropped, next_stab_cropped, output);
 
@@ -98,23 +100,14 @@ class Traite_image {
 			prev = next.clone();
 		}
 
-		//int to string helper function
-		string intToString(int number){
-
-			//this function has a number input and string output
-			std::stringstream ss;
-			ss << number;
-			return ss.str();
-		}
 
-		void stabiliseImg(Mat prev, Mat cur, Mat &output){
+		void trackFeatures(Mat prev, Mat cur) {
 			Mat cur_grey, prev_grey;
 			cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
 			cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
 
 			// vector from prev to cur
 			vector <Point2f> prev_corner, cur_corner;
-			vector <Point2f> prev_corner2, cur_corner2;
 			vector <uchar> status;
 			vector <float> err;
 
@@ -122,15 +115,18 @@ class Traite_image {
 			calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
 
 			// weed out bad matches
-			prev_ftr.clear();
-			cur_ftr.clear();
+			prev_ftr.resize(0);
+			cur_ftr.resize(0);
 			for(size_t i=0; i < status.size(); i++) {
 				if(status[i]) {
 					prev_ftr.push_back(prev_corner[i]);
 					cur_ftr.push_back(cur_corner[i]);
 				}
 			}
+		}
+
 
+		void stabiliseImg(Mat cur, Mat &output){
 			T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
 
 			if(T.data == NULL)
@@ -145,9 +141,11 @@ class Traite_image {
 			cur2.copyTo(output);
 		}
 
+
 		void warpPoints(vector<Point2f> p, vector<Point2f> &p_warp, Mat T, bool invert=false) {
+			Mat H;
 			if(invert)
-				invertAffineTransform(T, T);
+				invertAffineTransform(T, H);
 
 			p_warp.clear();
 			for(size_t i=0; i < p.size(); ++i) {
@@ -157,29 +155,34 @@ class Traite_image {
 				src.at<double>(1,0)=p[i].y;
 				src.at<double>(2,0)=1.0;
 
-				Mat dst = T*src; //USE MATRIX ALGEBRA
+				Mat dst = H*src; //USE MATRIX ALGEBRA
 				p_warp.push_back(Point2f(dst.at<double>(0,0),dst.at<double>(1,0)));
 			}
 		}
 
+
 		void trackingOptFlow(Mat prev, Mat cur, Mat &output) {
 			cur.copyTo(output);
 			vector <Point2f> cur_ftr_stab;
 
-			Mat T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
-			if(T.data == NULL)
-				last_T.copyTo(T);
-			else
-				T.copyTo(last_T);
+			//T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
+			//if(T.data == NULL)
+			//	last_T.copyTo(T);
+			//else
+			//	T.copyTo(last_T);
 
 			warpPoints(cur_ftr, cur_ftr_stab, T, true);
 
 			vector <Point2f> objects;
+			vector <float> flow_norm;
 			for(size_t i=0; i < prev_ftr.size(); ++i) {
-				float flow_norm = norm(prev_ftr[i] - cur_ftr_stab[i]) / prev.size().height;
+				flow_norm.push_back(norm(prev_ftr[i] - cur_ftr_stab[i]) / prev.size().height);
 				line(output, prev_ftr[i], cur_ftr[i], Scalar(200,0,0),1);
 				line(output, prev_ftr[i], cur_ftr_stab[i], Scalar(0,200,0),1);
-				if(flow_norm > MOVEMENT_THRES) {
+			}
+
+			for(size_t i=0; i < flow_norm.size(); ++i) {
+				if(flow_norm[i] > MOVEMENT_THRES) {
 					objects.push_back(cur_ftr_stab[i]);
 					prev_ftr.erase(prev_ftr.begin() + i);
 					cur_ftr.erase(cur_ftr.begin() + i);
@@ -191,6 +194,7 @@ class Traite_image {
 			}
 		}
 
+
 		inline bool isFlowCorrect(Point2f u)
 		{
 			return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;
@@ -215,6 +219,16 @@ class Traite_image {
 
             pub_cmd.publish(twist);
         }
+
+
+		//int to string helper function
+		string intToString(int number){
+
+			//this function has a number input and string output
+			std::stringstream ss;
+			ss << number;
+			return ss.str();
+		}
 };