A bit of cleaning, simul arg

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,8 +14,10 @@ using namespace std;
 
 class Traite_image {
 	public:
-		const static int SENSITIVITY_VALUE = 40;
-		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;
+		const static int crop_ratio = 8;
 
 
 		Mat prev;
@@ -33,10 +36,22 @@ class Traite_image {
 		image_transport::Subscriber     sub;
 
 
-		Traite_image() : n("~"),it(n) {
-			pub_img = it.advertise("/image_out", 1);
-			pub_cmd = n.advertise<geometry_msgs::Twist>("/vrep/drone/cmd_vel", 1);
-			sub = it.subscribe("/usb_cam/image_raw", 1, [this](const sensor_msgs::ImageConstPtr& img) -> void { this->on_image(img);},ros::VoidPtr(),image_transport::TransportHints("compressed"));
+		Traite_image(bool sim) : n("~"),it(n) {
+			String img_out, cmd_out, img_in;
+			if (!sim) {
+				img_out = "/image_out";
+				cmd_out = "/bebop/cmd_vel";
+				img_in = "/bebop/image_raw";
+			}
+			else
+			{
+				img_out = "/image_out";
+				cmd_out = "/vrep/drone/cmd_vel";
+				img_in = "/usb_cam/image_raw";
+			}
+			pub_img = it.advertise(img_out, 1);
+			pub_cmd = n.advertise<geometry_msgs::Twist>(cmd_out, 1);
+			sub = it.subscribe(img_in, 1, [this](const sensor_msgs::ImageConstPtr& img) -> void { this->on_image(img);},ros::VoidPtr(),image_transport::TransportHints("compressed"));
 		}
 
 
@@ -67,7 +82,6 @@ class Traite_image {
 
 			Mat next_stab;
 			stabiliseImg(prev, next, next_stab);
-			int crop_ratio = 6;
 			float crop_x = next_stab.size().width/crop_ratio;
 			float crop_y = next_stab.size().height/crop_ratio;
 			float crop_w = next_stab.size().width*(1-2.0/crop_ratio);
@@ -75,7 +89,7 @@ class Traite_image {
 			Rect myROI(crop_x, crop_y, crop_w, crop_h);
 			Mat next_stab_cropped = next_stab(myROI);
 			Mat prev_cropped = prev(myROI);
-			searchForMovement(prev_cropped, next_stab_cropped, output);
+			searchForMovementOptFlow(prev_cropped, next_stab_cropped, output);
 
 
 			pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
@@ -142,10 +156,10 @@ class Traite_image {
 			// Subtract the 2 last frames and threshold them
 			Mat thres;
 			absdiff(prev_grey,cur_grey,thres);
-			threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
+			threshold(thres, thres, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY);
 			// Blur to eliminate noise
 			blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE));
-			threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
+			threshold(thres, thres, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY);
 
 			//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.
@@ -186,6 +200,57 @@ class Traite_image {
 			putText(output,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2);
 		}
 
+        void searchForMovementOptFlow(Mat prev, Mat cur, Mat &output){
+			Mat cur_grey, prev_grey;
+			cur.copyTo(output);
+			cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
+			cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
+
+            Mat flow;
+            calcOpticalFlowFarneback(prev_grey, cur_grey, flow, 0.5, 3, 15, 3, 5, 1.2, 0);
+            vector<Mat> flow_coord(2);
+            Mat flow_norm, angle;
+            split(flow, flow_coord);
+            cartToPolar(flow_coord[0], flow_coord[1], flow_norm, angle);
+
+            //threshold(flow_norm, flow_norm, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY);
+			// Blur to eliminate noise
+			blur(flow_norm, flow_norm, Size(BLUR_SIZE, BLUR_SIZE));
+			threshold(flow_norm, flow_norm, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY);
+            flow_norm.convertTo(flow_norm, CV_8U);
+
+			Mat temp;
+			flow_norm.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
+
+			if(contours.size()>0){ //if contours vector is not empty, we have found some objects
+				//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));
+			}
+			//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);
+
+        }
+
 		inline bool isFlowCorrect(Point2f u)
 		{
 			return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;
@@ -199,11 +264,11 @@ 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;
             }
@@ -215,8 +280,9 @@ class Traite_image {
 
 int main(int argc, char **argv)
 {
-	ros::init(argc, argv, "test_opencv");
-	Traite_image dataset=Traite_image();
+	ros::init(argc, argv, "Papillon");
+	bool sim = false;
+	Traite_image dataset=Traite_image(sim);
 	ros::spin();
 
 	return 0;