Commands cheatsheet

CMakeLists.txt

@@ -23,6 +23,7 @@ target_link_libraries(papillon ${catkin_LIBRARIES})
 set_property         (TARGET papillon APPEND PROPERTY INCLUDE_DIRECTORIES ${OpenCV_INCLUDE_DIRS})
 set_property         (TARGET papillon APPEND PROPERTY INCLUDE_DIRECTORIES ${catkin_INCLUDE_DIRS})
 set_property         (TARGET papillon APPEND PROPERTY LINK_LIBRARIES      ${OpenCV_LIBRARIES})
+set( CMAKE_EXPORT_COMPILE_COMMANDS 1 )
 
 
 install(TARGETS papillon DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

commands

@@ -0,0 +1 @@
+catkin_make -DCMAKE_BUILD_TYPE=Debug

src/papillon.cpp

@@ -3,6 +3,7 @@
 #include <cv_bridge/cv_bridge.h>
 #include <sensor_msgs/image_encodings.h>
 #include <geometry_msgs/Twist.h>
+#include <typeinfo>
 
 #include <opencv/cv.h>
 
@@ -13,13 +14,26 @@ using namespace std;
 
 class Traite_image {
 	public:
-		const static int SENSITIVITY_VALUE = 30;
-		const static int BLUR_SIZE = 10;
+		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.05;
+
+		constexpr static float FLOW_MIN_QUAL = 0.01;
+		const static int FLOW_MIN_DIST = 20;
 
 
 		Mat prev;
-		Mat last_T;
+
+		// Stabilisation transformation matrices
+		Mat T, last_T;
+
 		bool first = true;
+
+		// Features vectors
+		vector <Point2f> prev_ftr, cur_ftr;
+
+		// Downsize factor
 		int resize_f = 1;
 
 		int theObject[2] = {0,0};
@@ -66,11 +80,14 @@ class Traite_image {
 			}
 
 			Mat next_stab;
-			stabiliseImg(prev, next, next_stab);
-            Rect myROI(next_stab.size().width/8, next_stab.size().height/8, next_stab.size().width*3/4, next_stab.size().height*3/4);
-            Mat next_stab_cropped = next_stab(myROI);
-            Mat prev_cropped = prev(myROI);
-			searchForMovement(prev_cropped, next_stab_cropped, output);
+			trackFeatures(prev, next);
+			stabiliseImg(next, next_stab);
+			trackingOptFlow(prev, next_stab, next_stab);
+			Mat next_stab2;
+			trackFeatures(prev, next);
+			stabiliseImg(next, next_stab2);
+			trackingOptFlow(prev, next_stab2, output);
+			//searchForMovementOptFlow(prev_cropped, next_stab_cropped, output);
 
 
 			pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
@@ -83,42 +100,38 @@ 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;
 
-			goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30);
+			goodFeaturesToTrack(prev_grey, prev_corner, 200, FLOW_MIN_QUAL, FLOW_MIN_DIST);
 			calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
 
 			// weed out bad matches
+			prev_ftr.resize(0);
+			cur_ftr.resize(0);
 			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]);
+					prev_ftr.push_back(prev_corner[i]);
+					cur_ftr.push_back(cur_corner[i]);
+				}
 			}
 		}
 
-			Mat T = estimateRigidTransform(prev_corner2, cur_corner2, true); // false = rigid transform, no scaling/shearing
 
-			if(T.data == NULL) {
+		void stabiliseImg(Mat cur, Mat &output){
+			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);
 
 			Mat cur2;
@@ -128,58 +141,59 @@ class Traite_image {
 			cur2.copyTo(output);
 		}
 
-		void searchForMovement(Mat prev, Mat cur, Mat &output){
-			Mat cur_grey, prev_grey;
+
+		void warpPoints(vector<Point2f> p, vector<Point2f> &p_warp, Mat T, bool invert=false) {
+			Mat H;
+			if(invert)
+				invertAffineTransform(T, H);
+
+			p_warp.clear();
+			for(size_t i=0; i < p.size(); ++i) {
+				Mat src(3/*rows*/,1 /* cols */,CV_64F);
+
+				src.at<double>(0,0)=p[i].x;
+				src.at<double>(1,0)=p[i].y;
+				src.at<double>(2,0)=1.0;
+
+				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);
-			cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
-			cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
+			vector <Point2f> cur_ftr_stab;
 
-			// Subtract the 2 last frames and threshold them
-			Mat thres;
-			absdiff(prev_grey,cur_grey,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);
+			//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);
 
-			//notice how we use the '&' operator for objectDetected and output. This is because we wish
-			//to take the values passed into the function and manipulate them, rather than just working with a copy.
-			//eg. we draw to the output to be displayed in the main() function.
-			bool objectDetected = false;
-			Mat temp;
-			thres.copyTo(temp);
-			//these two vectors needed for output of findContours
-			vector< vector<Point> > contours;
-			vector<Vec4i> hierarchy;
-			//find contours of filtered image using openCV findContours function
-			//findContours(temp,contours,hierarchy,CV_RETR_CCOMP,CV_CHAIN_APPROX_SIMPLE );// retrieves all contours
-			findContours(temp,contours,hierarchy,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE );// retrieves external contours
+			warpPoints(cur_ftr, cur_ftr_stab, T, true);
 
-			//if contours vector is not empty, we have found some objects
-			if(contours.size()>0)objectDetected=true;
-			else objectDetected = false;
-
-			if(objectDetected){
-				//the largest contour is found at the end of the contours vector
-				//we will simply assume that the biggest contour is the object we are looking for.
-				vector< vector<Point> > largestContourVec;
-				largestContourVec.push_back(contours.at(contours.size()-1));
-				//make a bounding rectangle around the largest contour then find its centroid
-				//this will be the object's final estimated position.
-				objectBoundingRectangle = boundingRect(largestContourVec.at(0));
+			vector <Point2f> objects;
+			vector <float> flow_norm;
+			for(size_t i=0; i < prev_ftr.size(); ++i) {
+				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);
 			}
-			//make some temp x and y variables so we dont have to type out so much
-			int x = objectBoundingRectangle.x;
-			int y = objectBoundingRectangle.y;
-			int width = objectBoundingRectangle.width;
-			int height = objectBoundingRectangle.height;
 
-			//draw a rectangle around the object
-			rectangle(output, Point(x,y), Point(x+width, y+height), Scalar(0, 255, 0), 2);
-
-			//write the position of the object to the screen
-			putText(output,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2);
+			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);
 				}
+			}
+
+			for(size_t i=0; i < objects.size(); ++i) {
+				circle(output, objects[i], 5, Scalar(0, 200, 0), 1);
+			}
+		}
+
 
 		inline bool isFlowCorrect(Point2f u)
 		{
@@ -194,17 +208,27 @@ class Traite_image {
 
             geometry_msgs::Twist twist = geometry_msgs::Twist();
 
-            if(centre_rect.x < centre_image.x)
+            if(centre_rect.x < centre_image.x-THRESHOLD_MOV)
             {
                 twist.angular.z = 0.2;
             }
-            else if(centre_rect.x > centre_image.x)
+            else if(centre_rect.x > centre_image.x+THRESHOLD_MOV)
             {
                 twist.angular.z = -0.2;
             }
 
             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();
+		}
 };