Remove unused variable

CMakeLists.txt

@@ -6,20 +6,37 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   image_transport
   cv_bridge
+  message_generation
 )
 find_package(OpenCV)
+find_package( PkgConfig REQUIRED )
+pkg_check_modules ( ncurses++ REQUIRED ncurses++ )
 
+add_message_files(
+  FILES
+  BoundingBox.msg
+)
+
+generate_messages(
+    DEPENDENCIES
+    std_msgs
+)
 catkin_package(CATKIN_DEPENDS
   roscpp
   std_msgs
   image_transport
   cv_bridge
+  message_runtime
 )
 
+
 include_directories  (${catkin_INCLUDE_DIRS})
 
 add_executable       (papillon src/papillon.cpp)
+add_executable       (control src/control.cpp)
+add_dependencies( papillon ${PROJECT_NAME}_generate_messages_cpp )
 target_link_libraries(papillon ${catkin_LIBRARIES})
+target_link_libraries(control ${catkin_LIBRARIES} ncurses)
 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})

README.md

@@ -1,3 +1,22 @@
 # Papillon
 
-## Comportements émergents avec les drônes bebop
+Comportements émergents avec les drônes bebop
+
+## Utilisation
+
+Il faut en premier lieu se connecter en WiFi au drone.
+
+Il faut ensuite bien entendu lancer `roscore`.
+
+Une fois ce dernier lancé, on peut lancer le driver du drone afin qu'il soit connecté à ROS à l'aide de la commande :
+```bash
+  $ roslaunch bebop_driver bebop_node.launch
+```
+Il faut ensuite lancer les deux noeuds de `papillon` à l'aide des commandes :
+```bash
+  $ rosrun papillon papillon
+  $ rosrun papillon control
+```
+Enfin, sur le noeud `control`, lors de l'appui sur la touche **F** le drone décolle et commence à suivre les mouvements qu'il détecte.
+
+Lors d'un appui sur la touche **Espace**, le drone arrête de suivre et se pose.

commandes

@@ -0,0 +1,6 @@
+rosrun demo_teleop key_teleop.py --persist /cmd_vel:=/bebop/cmd_vel
+rostopic pub --once /bebop/land std_msgs/Empty
+rostopic pub --once /bebop/takeoff std_msgs/Empty
+catkin_make --pkg=papillon
+catkin_make -DCMAKE_BUILD_TYPE=Debug
+roslaunch bebop_driver bebop_node.launch

msg/BoundingBox.msg

@@ -0,0 +1,4 @@
+float64 x
+float64 y
+float64 width
+float64 height

package.xml

@@ -45,10 +45,13 @@
   <build_depend>std_msgs</build_depend>
   <build_depend>image_transport</build_depend>
   <build_depend>cv_bridge</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>ncurses++</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>std_msgs</run_depend>
   <run_depend>image_transport</run_depend>
   <run_depend>cv_bridge</run_depend>
+  <run_depend>message_runtime</run_depend>
 
 
 

src/control.cpp

@@ -0,0 +1,137 @@
+#include "ros/ros.h"
+#include <papillon/BoundingBox.h>
+#include <geometry_msgs/Twist.h>
+#include <std_msgs/Empty.h>
+#include <ncurses.h>
+
+#include <opencv/cv.h>
+
+#include <sstream>
+
+using namespace std;
+
+class Drone_control {
+	public:
+		constexpr static float MAX_ANG_VEL = 0.3;
+
+		ros::NodeHandle n;
+		ros::Publisher pub_cmd;
+		ros::Publisher pub_takeoff;
+		ros::Publisher pub_land;
+		ros::Subscriber sub_box;
+		std_msgs::Empty nope;
+
+		float THRES_TURN = 0.1;
+		bool emergency = false;
+
+		// 1 left | 0 center | -1 right
+		vector<int> decision_acc;
+		const static int NB_ACC = 5;
+
+
+		Drone_control() : n("~") {
+			pub_cmd = n.advertise<geometry_msgs::Twist>("/bebop/cmd_vel", 1);
+			pub_takeoff = n.advertise<std_msgs::Empty>("/bebop/takeoff", 1);
+			pub_land = n.advertise<std_msgs::Empty>("/bebop/land", 1);
+			sub_box = n.subscribe<papillon::BoundingBox>("/papillon/bbox", 1, &Drone_control::on_msg, this);
+
+			// Initialise decision accumulator
+			for (size_t i = 0; i < NB_ACC; ++i)
+				decision_acc.push_back(0);
+
+			ROS_INFO("Built !");
+		}
+
+
+		// This processes an image and publishes the result.
+		void on_msg(const papillon::BoundingBox::ConstPtr& bbox) {
+			geometry_msgs::Twist twist = geometry_msgs::Twist();
+			cv::Point2f r_center = cv::Point2f(bbox->x + bbox->width/2, bbox->y + bbox->height/2);
+			if (!emergency) {
+				decision_acc.pop_back();
+				// Image is 1.0 in width and height
+				if (r_center.x < 0.5 - THRES_TURN)
+					decision_acc.insert(decision_acc.begin(), 1);
+				else if (r_center.x > 0.5 + THRES_TURN)
+					decision_acc.insert(decision_acc.begin(), -1);
+				else
+					decision_acc.insert(decision_acc.begin(), 0);
+
+				int moy = 0;
+				for (const auto& d : decision_acc)
+					moy += d;
+				twist.angular.z = moy / (float)NB_ACC * MAX_ANG_VEL;
+			}
+
+			pub_cmd.publish(twist);
+		}
+
+		void key_spin() {
+			// >>>>> nCurses initization
+			initscr();
+			keypad(stdscr, TRUE);
+			cbreak();
+			noecho();
+			timeout(0); // Non blocking getch()
+			// <<<<< nCurses initization
+
+			int c;
+
+			printw(" ------------------------\n");
+			printw("|      Papillon HQ       |\n");
+			printw(" ------------------------ \n");
+			printw("| Press F to take off.   |\n");
+			printw("| Press SPACEBAR to land.|\n");
+			printw("| Press Q to exit.       |\n");
+			printw(" ------------------------\n");
+
+			bool stop = false;
+
+			ros::Rate rate(1.0/30.0);  // 30 Hz
+			while(!stop)
+			{
+				c = getch();
+
+				switch(c)
+				{
+				case 'f':
+				{
+					// Takeoff
+					ROS_DEBUG_STREAM("Taking off...\r");
+					pub_takeoff.publish(nope);
+					emergency = false;
+					printw("Taking off...\r");
+					break;
+				}
+				case ' ':
+				{
+					// Takeoff
+					ROS_DEBUG_STREAM("Landing...\r");
+					pub_land.publish(nope);
+					printw("Landing...\r");
+					emergency = true;
+					break;
+				}
+				case 'q':
+				case 'Q':
+				{
+					ROS_DEBUG_STREAM("EXIT\r");
+					stop = true;
+				}
+				}
+				ros::spinOnce();
+			}
+			printw("\nExiting\n");
+		}
+};
+
+
+int main(int argc, char **argv)
+{
+	ros::init(argc, argv, "control");
+	Drone_control con=Drone_control();
+	con.key_spin();
+
+	return 0;
+}
+

src/papillon.cpp

@@ -2,128 +2,105 @@
 #include <image_transport/image_transport.h>
 #include <cv_bridge/cv_bridge.h>
 #include <sensor_msgs/image_encodings.h>
-#include <geometry_msgs/Twist.h>
+#include <papillon/BoundingBox.h>
-#include <typeinfo>
 
 #include <opencv/cv.h>
 
 #include <sstream>
 
-using namespace cv;
 using namespace std;
 
 class Traite_image {
 	public:
-		const static int THRESHOLD_DETECT_SENSITIVITY = 10;
+		const static int SENSITIVITY_VALUE = 20;
-		const static int BLUR_SIZE = 5;
+		const static int BLUR_SIZE = 9;
-        const static int THRESHOLD_MOV = 5;
+		const static int CLOSE_SIZE = 30;
-		const static int crop_ratio = 8;
+		const static int ERODE_SIZE = 2;
+		const static int NB_FRAME_DROP = 1;
 
 
-		Mat prev;
+		vector<cv::Mat> prevs;
-		Mat last_T;
+		cv::Mat last_T;
 		bool first = true;
 		int resize_f = 1;
 
-		int theObject[2] = {0,0};
-		Rect objectBoundingRectangle = Rect(0,0,0,0);
-
 		ros::NodeHandle n;
 
 		image_transport::ImageTransport it;
-		image_transport::Publisher      pub_img;
-        ros::Publisher                pub_cmd;
 		image_transport::Subscriber     sub;
+		image_transport::Publisher      pub_img;
+		image_transport::Publisher      pub_thres;
+		ros::Publisher                  pub_cmd;
 
 
-		Traite_image(bool sim) : n("~"),it(n) {
+		Traite_image() : n("~"),it(n) {
-			String img_out, cmd_out, img_in;
+			pub_img = it.advertise("/papillon/image_out", 1);
-			if (!sim) {
+			pub_thres = it.advertise("/papillon/thres_out", 1);
-				img_out = "/image_out";
+			pub_cmd = n.advertise<papillon::BoundingBox>("/papillon/bbox", 1);
-				cmd_out = "/bebop/cmd_vel";
+			sub = it.subscribe("/bebop/image_raw", 1, [this](const sensor_msgs::ImageConstPtr& img) -> void { this->on_image(img);},ros::VoidPtr(),image_transport::TransportHints("compressed"));
-				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"));
 		}
 
 
-		// This processes an image and publishes the result.
+		// Callback function executedwhen recieving an image from the camera
+		// Publishes to /papillon/image_out
 		void on_image(const sensor_msgs::ImageConstPtr& msg) {
 
 			cv_bridge::CvImageConstPtr bridge_input;
 			try {
 				bridge_input = cv_bridge::toCvShare(msg,sensor_msgs::image_encodings::RGB8);
 			}
-			catch (Exception& e) {
+			catch (cv::Exception& e) {
 				std::ostringstream errstr;
 				errstr << "cv_bridge exception caught: " << e.what();
 				return;
 			}
 
-			//Mat& input  = const_cast<Mat&>(bridge_input->image);
+			const cv::Mat& input = bridge_input->image;
-			const Mat& input = bridge_input->image;
+			cv::Mat next;
-			Mat next;
+			resize(input, next, cv::Size(input.size().width/resize_f, input.size().height/resize_f));
-			resize(input, next, Size(input.size().width/resize_f, input.size().height/resize_f));
+			cv::Mat output;
-			Mat output;// = input.clone(); //   (input.rows, input.cols, CV_32FC2);
+			next.copyTo(output);
-			//ROS_INFO("got input");
+			
+			cv::cvtColor(next, next, cv::COLOR_BGR2GRAY);
+			
 			if (first) {
-				prev = next.clone();
+				for (int i = 0; i < NB_FRAME_DROP; ++i) {
+					prevs.push_back(next.clone());
+				}
 				first = false;
-				ROS_INFO("first done");
+				ROS_INFO("Ready");
 			}
 
-			Mat next_stab;
+			cv::Mat next_stab;
-			stabiliseImg(prev, next, next_stab);
+			stabiliseImg(prevs.back(), 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);
 			float crop_h = next_stab.size().height*(1-2.0/crop_ratio);
-			Rect myROI(crop_x, crop_y, crop_w, crop_h);
+			cv::Rect myROI(crop_x, crop_y, crop_w, crop_h);
-			Mat next_stab_cropped = next_stab(myROI);
+			cv::Mat next_stab_cropped = next_stab(myROI);
-			Mat prev_cropped = prev(myROI);
+			cv::Mat prev_cropped = prevs.back()(myROI);
-			searchForMovementOptFlow(prev_cropped, next_stab_cropped, output);
+			cv::Mat closed_thres;
+			searchForMovement(prev_cropped, next_stab_cropped, output, closed_thres);
 
 
 			pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
-			// bridge_input is handled by a smart-pointer. No explicit delete needed.
+			pub_thres.publish(cv_bridge::CvImage(msg->header, "mono8", closed_thres).toImageMsg());
 
-            //droneTracking(Rect(Point(0,0), output.size()));
+			prevs.pop_back();
-
+			prevs.insert(prevs.begin(), next.clone());
-			//ROS_INFO("pub");
-
-			prev = next.clone();
 		}
 
-		//int to string helper function
+		void stabiliseImg(cv::Mat prev_grey, cv::Mat cur_grey, cv::Mat &output){
-		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){
-			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 <cv::Point2f> prev_corner, cur_corner;
-			vector <Point2f> prev_corner2, cur_corner2;
+			vector <cv::Point2f> prev_corner2, cur_corner2;
 			vector <uchar> status;
 			vector <float> err;
 
-			goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30);
+			cv::goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30);
-			calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
+			cv::calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
 
 			// weed out bad matches
 			for(size_t i=0; i < status.size(); i++) {
@@ -133,156 +110,106 @@ class Traite_image {
 				}
 			}
 
-			Mat T = estimateRigidTransform(prev_corner2, cur_corner2, true); // false = rigid transform, no scaling/shearing
+			cv::Mat T = estimateRigidTransform(prev_corner2, cur_corner2, true); // false = rigid transform, no scaling/shearing
 
 			if(T.data == NULL) {
 				last_T.copyTo(T);
 			}
 			T.copyTo(last_T);
 
-			Mat cur2;
+			cv::warpAffine(cur_grey, output, T, cur_grey.size(),cv::INTER_CUBIC|cv::WARP_INVERSE_MAP);
-
-			warpAffine(cur, cur2, T, cur.size(),INTER_NEAREST|WARP_INVERSE_MAP);
-
-			cur2.copyTo(output);
 		}
 
-		void searchForMovement(Mat prev, Mat cur, Mat &output){
+		void searchForMovement(cv::Mat prev_grey, cv::Mat cur_grey, cv::Mat &output, cv::Mat &out2){
-			Mat cur_grey, prev_grey;
+			cv::GaussianBlur(prev_grey, prev_grey, cv::Size(BLUR_SIZE,BLUR_SIZE), 3.0);
-			cur.copyTo(output);
+			cv::GaussianBlur(cur_grey, cur_grey, cv::Size(BLUR_SIZE,BLUR_SIZE), 3.0);
-			cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
-			cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
 
 			// Subtract the 2 last frames and threshold them
-			Mat thres;
+			cv::Mat thres;
-			absdiff(prev_grey,cur_grey,thres);
+			cv::absdiff(prev_grey,cur_grey,thres);
-			threshold(thres, thres, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY);
+
-			// Blur to eliminate noise
+			cv::Mat element = getStructuringElement( cv::MORPH_ELLIPSE,
-			blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE));
+                                   cv::Size( 2*ERODE_SIZE + 1, 2*ERODE_SIZE+1 ),
-			threshold(thres, thres, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY);
+                                   cv::Point( ERODE_SIZE, ERODE_SIZE ) );
+			// Apply the erode operation
+			cv::erode(thres, thres, element );
+
+			cv::threshold(thres, thres, SENSITIVITY_VALUE, 255, cv::THRESH_BINARY);
+
+			// Intermediate output
+			thres.copyTo(out2);
+
+			cv::Mat closed_thres;
+			cv::Mat structuringElement = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(CLOSE_SIZE, CLOSE_SIZE));
+			cv::morphologyEx( thres, closed_thres, cv::MORPH_CLOSE, structuringElement );
 
-			//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;
+			cv::Mat temp;
-			thres.copyTo(temp);
+			closed_thres.copyTo(temp);
-			//these two vectors needed for output of findContours
+			vector< vector<cv::Point> > contours;
-			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
+			cv::findContours(temp,contours,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE );// retrieves external contours
-			findContours(temp,contours,hierarchy,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE );// retrieves external contours
 
 			//if contours vector is not empty, we have found some objects
-			if(contours.size()>0)objectDetected=true;
+			if(contours.size()>0){
-			else objectDetected = false;
+				vector<cv::Rect> nc_rects; // Non connected rectangles
-
+				for(size_t i=0; i<contours.size();i++)
-			if(objectDetected){
+				{
-				//the largest contour is found at the end of the contours vector
+					nc_rects.push_back(cv::boundingRect(contours[i]));
-				//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);
 				}
 
-        void searchForMovementOptFlow(Mat prev, Mat cur, Mat &output){
+				vector<cv::Rect> c_rects; // Connected rectangles
-			Mat cur_grey, prev_grey;
+				cleanBBoxes(nc_rects, cur_grey.size(), c_rects);
-			cur.copyTo(output);
+				if (c_rects.size() > 0) {
-			cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
+					for (const auto& rect : c_rects)
-			cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
+						cv::rectangle(output, rect, cv::Scalar(0, 255, 0), 2);
 
-            Mat flow;
+					cv::Rect objBRect = c_rects.front();
-            calcOpticalFlowFarneback(prev_grey, cur_grey, flow, 0.5, 3, 15, 3, 5, 1.2, 0);
+					papillon::BoundingBox bbox = papillon::BoundingBox();
-            vector<Mat> flow_coord(2);
+					bbox.x = objBRect.x / (float)cur_grey.size().width;
-            Mat flow_norm, angle;
+					bbox.y = objBRect.y / (float)cur_grey.size().height;
-            split(flow, flow_coord);
+					bbox.width = objBRect.width / (float)cur_grey.size().width;
-            cartToPolar(flow_coord[0], flow_coord[1], flow_norm, angle);
+					bbox.height = objBRect.height / (float)cur_grey.size().height;
-
+					pub_cmd.publish(bbox);
-            //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)
+		void cleanBBoxes(vector<cv::Rect> nc_rects, cv::Size img, vector<cv::Rect> &c_rects) {
+			int max = 0;
+			for (auto r : nc_rects) {
+				cv::Point center = rectCenter(r);
+				if (r.width > img.width / 2 || r.height > img.height / 8)
+					continue;
+
+				if (center.y > img.height * 2 / 3 || center.y < img.height / 3)
+					continue;
+
+				int r_area = r.area();
+				if (max < r_area) {
+					max = r_area;
+					c_rects.insert(c_rects.begin(), r);
+				} else {
+					c_rects.push_back(r);
+				}
+			}
+		}
+
+		cv::Point rectCenter(cv::Rect r) {
+			return cv::Point(r.x + r.width/2, r.y + r.height / 2);
+		}
+
+		inline bool isFlowCorrect(cv::Point2f u)
 		{
 			return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;
 		}
-
-
-        void droneTracking(Rect img_size)
-        {
-            Point2f centre_image = Point2f(img_size.width/2, img_size.height/2);
-            Point2f centre_rect = Point2f(objectBoundingRectangle.x + objectBoundingRectangle.width/2, objectBoundingRectangle.y + objectBoundingRectangle.height/2);
-
-            geometry_msgs::Twist twist = geometry_msgs::Twist();
-
-            if(centre_rect.x < centre_image.x-THRESHOLD_MOV)
-            {
-                twist.angular.z = 0.2;
-            }
-            else if(centre_rect.x > centre_image.x+THRESHOLD_MOV)
-            {
-                twist.angular.z = -0.2;
-            }
-
-            pub_cmd.publish(twist);
-        }
 };
 
 
 int main(int argc, char **argv)
 {
-	ros::init(argc, argv, "Papillon");
+	ros::init(argc, argv, "test_opencv");
-	bool sim = false;
+	Traite_image dataset=Traite_image();
-	Traite_image dataset=Traite_image(sim);
 	ros::spin();
 
 	return 0;

src/videostab.cpp

@@ -1,258 +0,0 @@
-#include <opencv2/opencv.hpp>
-#include <iostream>
-#include <cassert>
-#include <cmath>
-#include <fstream>
-
-using namespace std;
-using namespace cv;
-
-// This video stablisation smooths the global trajectory using a sliding average window
-
-const int SMOOTHING_RADIUS = 30; // In frames. The larger the more stable the video, but less reactive to sudden panning
-const int HORIZONTAL_BORDER_CROP = 20; // In pixels. Crops the border to reduce the black borders from stabilisation being too noticeable.
-
-// 1. Get previous to current frame transformation (dx, dy, da) for all frames
-// 2. Accumulate the transformations to get the image trajectory
-// 3. Smooth out the trajectory using an averaging window
-// 4. Generate new set of previous to current transform, such that the trajectory ends up being the same as the smoothed trajectory
-// 5. Apply the new transformation to the video
-
-struct TransformParam
-{
-    TransformParam() {}
-    TransformParam(double _dx, double _dy, double _da) {
-        dx = _dx;
-        dy = _dy;
-        da = _da;
-    }
-
-    double dx;
-    double dy;
-    double da; // angle
-};
-
-struct Trajectory
-{
-    Trajectory() {}
-    Trajectory(double _x, double _y, double _a) {
-        x = _x;
-        y = _y;
-        a = _a;
-    }
-
-    double x;
-    double y;
-    double a; // angle
-};
-
-int main(int argc, char **argv)
-{
-    if(argc < 2) {
-        cout << "./VideoStab [video.avi]" << endl;
-        return 0;
-    }
-
-    // For further analysis
-    ofstream out_transform("prev_to_cur_transformation.txt");
-    ofstream out_trajectory("trajectory.txt");
-    ofstream out_smoothed_trajectory("smoothed_trajectory.txt");
-    ofstream out_new_transform("new_prev_to_cur_transformation.txt");
-
-    VideoCapture cap(argv[1]);
-    assert(cap.isOpened());
-
-    Mat cur, cur_grey;
-    Mat prev, prev_grey;
-
-    cap >> prev;
-    cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
-
-    // Step 1 - Get previous to current frame transformation (dx, dy, da) for all frames
-    vector <TransformParam> prev_to_cur_transform; // previous to current
-
-    int k=1;
-    int max_frames = cap.get(CV_CAP_PROP_FRAME_COUNT);
-    Mat last_T;
-
-    while(true) {
-        cap >> cur;
-
-        if(cur.data == NULL) {
-            break;
-        }
-
-        cvtColor(cur, cur_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);
-        calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
-
-        // weed out bad matches
-        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]);
-            }
-        }
-
-        // translation + rotation only
-        Mat T = estimateRigidTransform(prev_corner2, cur_corner2, false); // false = rigid transform, no scaling/shearing
-
-        // 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);
-
-        // 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));
-
-        prev_to_cur_transform.push_back(TransformParam(dx, dy, da));
-
-        out_transform << k << " " << dx << " " << dy << " " << da << endl;
-
-        cur.copyTo(prev);
-        cur_grey.copyTo(prev_grey);
-
-        cout << "Frame: " << k << "/" << max_frames << " - good optical flow: " << prev_corner2.size() << endl;
-        k++;
-    }
-
-    // Step 2 - Accumulate the transformations to get the image trajectory
-
-    // Accumulated frame to frame transform
-    double a = 0;
-    double x = 0;
-    double y = 0;
-
-    vector <Trajectory> trajectory; // trajectory at all frames
-
-    for(size_t i=0; i < prev_to_cur_transform.size(); i++) {
-        x += prev_to_cur_transform[i].dx;
-        y += prev_to_cur_transform[i].dy;
-        a += prev_to_cur_transform[i].da;
-
-        trajectory.push_back(Trajectory(x,y,a));
-
-        out_trajectory << (i+1) << " " << x << " " << y << " " << a << endl;
-    }
-
-    // Step 3 - Smooth out the trajectory using an averaging window
-    vector <Trajectory> smoothed_trajectory; // trajectory at all frames
-
-    for(size_t i=0; i < trajectory.size(); i++) {
-        double sum_x = 0;
-        double sum_y = 0;
-        double sum_a = 0;
-        int count = 0;
-
-        for(int j=-SMOOTHING_RADIUS; j <= SMOOTHING_RADIUS; j++) {
-            if(i+j >= 0 && i+j < trajectory.size()) {
-                sum_x += trajectory[i+j].x;
-                sum_y += trajectory[i+j].y;
-                sum_a += trajectory[i+j].a;
-
-                count++;
-            }
-        }
-
-        double avg_a = sum_a / count;
-        double avg_x = sum_x / count;
-        double avg_y = sum_y / count;
-
-        smoothed_trajectory.push_back(Trajectory(avg_x, avg_y, avg_a));
-
-        out_smoothed_trajectory << (i+1) << " " << avg_x << " " << avg_y << " " << avg_a << endl;
-    }
-
-    // Step 4 - Generate new set of previous to current transform, such that the trajectory ends up being the same as the smoothed trajectory
-    vector <TransformParam> new_prev_to_cur_transform;
-
-    // Accumulated frame to frame transform
-    a = 0;
-    x = 0;
-    y = 0;
-
-    for(size_t i=0; i < prev_to_cur_transform.size(); i++) {
-        x += prev_to_cur_transform[i].dx;
-        y += prev_to_cur_transform[i].dy;
-        a += prev_to_cur_transform[i].da;
-
-        // target - current
-        double diff_x = smoothed_trajectory[i].x - x;
-        double diff_y = smoothed_trajectory[i].y - y;
-        double diff_a = smoothed_trajectory[i].a - a;
-
-        double dx = prev_to_cur_transform[i].dx + diff_x;
-        double dy = prev_to_cur_transform[i].dy + diff_y;
-        double da = prev_to_cur_transform[i].da + diff_a;
-
-        new_prev_to_cur_transform.push_back(TransformParam(dx, dy, da));
-
-        out_new_transform << (i+1) << " " << dx << " " << dy << " " << da << endl;
-    }
-
-    // Step 5 - Apply the new transformation to the video
-    cap.set(CV_CAP_PROP_POS_FRAMES, 0);
-    Mat T(2,3,CV_64F);
-
-    int vert_border = HORIZONTAL_BORDER_CROP * prev.rows / prev.cols; // get the aspect ratio correct
-
-    k=0;
-    while(k < max_frames-1) { // don't process the very last frame, no valid transform
-        cap >> cur;
-
-        if(cur.data == NULL) {
-            break;
-        }
-
-        T.at<double>(0,0) = cos(new_prev_to_cur_transform[k].da);
-        T.at<double>(0,1) = -sin(new_prev_to_cur_transform[k].da);
-        T.at<double>(1,0) = sin(new_prev_to_cur_transform[k].da);
-        T.at<double>(1,1) = cos(new_prev_to_cur_transform[k].da);
-
-        T.at<double>(0,2) = new_prev_to_cur_transform[k].dx;
-        T.at<double>(1,2) = new_prev_to_cur_transform[k].dy;
-
-        Mat cur2;
-
-        warpAffine(cur, cur2, T, cur.size());
-
-        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());
-
-        // Now draw the original and stablised side by side for coolness
-        Mat canvas = Mat::zeros(cur.rows, cur.cols*2+10, cur.type());
-
-        cur.copyTo(canvas(Range::all(), Range(0, cur2.cols)));
-        cur2.copyTo(canvas(Range::all(), Range(cur2.cols+10, cur2.cols*2+10)));
-
-        // If too big to fit on the screen, then scale it down by 2, hopefully it'll fit :)
-        if(canvas.cols > 1920) {
-            resize(canvas, canvas, Size(canvas.cols/2, canvas.rows/2));
-        }
-
-        imshow("before and after", canvas);
-
-        //char str[256];
-        //sprintf(str, "images/%08d.jpg", k);
-        //imwrite(str, canvas);
-
-        waitKey(20);
-
-        k++;
-    }
-
-    return 0;
-}