Compare commits
26 commits
drone_cont
...
master
Author | SHA1 | Date | |
---|---|---|---|
![]() |
a16b8903a1 | ||
![]() |
f440d0235f | ||
![]() |
9383d6db55 | ||
![]() |
09a5c786f8 | ||
![]() |
e18683fd42 | ||
![]() |
77d16e098a | ||
![]() |
5a08849467 | ||
![]() |
41e1ec9f2e | ||
![]() |
1d4447e29a | ||
![]() |
658cd286ba | ||
![]() |
210b762ffe | ||
![]() |
db2c5fb3bf | ||
![]() |
3038c41036 | ||
![]() |
284c115b1c | ||
![]() |
de402960c6 | ||
![]() |
69b74cfe60 | ||
![]() |
c8f38a0480 | ||
![]() |
d44ac1a76c | ||
![]() |
4c6a0b9478 | ||
![]() |
809c4d3ad2 | ||
![]() |
8fe8b8a1ca | ||
![]() |
28435564d6 | ||
![]() |
bdaba2e13c | ||
![]() |
0e85119c64 | ||
![]() |
ca53673111 | ||
![]() |
ff44b1eb13 |
8 changed files with 303 additions and 382 deletions
|
@ -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})
|
||||
|
|
21
README.md
21
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.
|
6
commandes
Normal file
6
commandes
Normal file
|
@ -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
|
4
msg/BoundingBox.msg
Normal file
4
msg/BoundingBox.msg
Normal file
|
@ -0,0 +1,4 @@
|
|||
float64 x
|
||||
float64 y
|
||||
float64 width
|
||||
float64 height
|
|
@ -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>
|
||||
|
||||
|
||||
|
||||
|
|
137
src/control.cpp
Normal file
137
src/control.cpp
Normal file
|
@ -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;
|
||||
}
|
||||
|
239
src/papillon.cpp
239
src/papillon.cpp
|
@ -2,114 +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 <opencv/cv.h>
|
||||
|
||||
#include <sstream>
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
class Traite_image {
|
||||
public:
|
||||
const static int SENSITIVITY_VALUE = 40;
|
||||
const static int BLUR_SIZE = 10;
|
||||
const static int SENSITIVITY_VALUE = 20;
|
||||
const static int BLUR_SIZE = 9;
|
||||
const static int CLOSE_SIZE = 30;
|
||||
const static int ERODE_SIZE = 2;
|
||||
const static int NB_FRAME_DROP = 1;
|
||||
|
||||
|
||||
Mat prev;
|
||||
Mat last_T;
|
||||
vector<cv::Mat> prevs;
|
||||
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() : 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"));
|
||||
pub_img = it.advertise("/papillon/image_out", 1);
|
||||
pub_thres = it.advertise("/papillon/thres_out", 1);
|
||||
pub_cmd = n.advertise<papillon::BoundingBox>("/papillon/bbox", 1);
|
||||
sub = it.subscribe("/bebop/image_raw", 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 Mat& input = bridge_input->image;
|
||||
Mat next;
|
||||
resize(input, next, Size(input.size().width/resize_f, input.size().height/resize_f));
|
||||
Mat output;// = input.clone(); // (input.rows, input.cols, CV_32FC2);
|
||||
//ROS_INFO("got input");
|
||||
const cv::Mat& input = bridge_input->image;
|
||||
cv::Mat next;
|
||||
resize(input, next, cv::Size(input.size().width/resize_f, input.size().height/resize_f));
|
||||
cv::Mat output;
|
||||
next.copyTo(output);
|
||||
|
||||
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;
|
||||
stabiliseImg(prev, next, next_stab);
|
||||
cv::Mat 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);
|
||||
Mat next_stab_cropped = next_stab(myROI);
|
||||
Mat prev_cropped = prev(myROI);
|
||||
searchForMovement(prev_cropped, next_stab_cropped, output);
|
||||
cv::Rect myROI(crop_x, crop_y, crop_w, crop_h);
|
||||
cv::Mat next_stab_cropped = next_stab(myROI);
|
||||
cv::Mat prev_cropped = prevs.back()(myROI);
|
||||
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()));
|
||||
|
||||
//ROS_INFO("pub");
|
||||
|
||||
prev = next.clone();
|
||||
prevs.pop_back();
|
||||
prevs.insert(prevs.begin(), 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){
|
||||
Mat cur_grey, prev_grey;
|
||||
cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
|
||||
cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
|
||||
|
||||
void stabiliseImg(cv::Mat prev_grey, cv::Mat cur_grey, cv::Mat &output){
|
||||
// vector from prev to cur
|
||||
vector <Point2f> prev_corner, cur_corner;
|
||||
vector <Point2f> prev_corner2, cur_corner2;
|
||||
vector <cv::Point2f> prev_corner, cur_corner;
|
||||
vector <cv::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);
|
||||
cv::goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30);
|
||||
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++) {
|
||||
|
@ -119,97 +110,99 @@ 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;
|
||||
|
||||
warpAffine(cur, cur2, T, cur.size(),INTER_NEAREST|WARP_INVERSE_MAP);
|
||||
|
||||
cur2.copyTo(output);
|
||||
cv::warpAffine(cur_grey, output, T, cur_grey.size(),cv::INTER_CUBIC|cv::WARP_INVERSE_MAP);
|
||||
}
|
||||
|
||||
void searchForMovement(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);
|
||||
void searchForMovement(cv::Mat prev_grey, cv::Mat cur_grey, cv::Mat &output, cv::Mat &out2){
|
||||
cv::GaussianBlur(prev_grey, prev_grey, cv::Size(BLUR_SIZE,BLUR_SIZE), 3.0);
|
||||
cv::GaussianBlur(cur_grey, cur_grey, cv::Size(BLUR_SIZE,BLUR_SIZE), 3.0);
|
||||
|
||||
// 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);
|
||||
cv::Mat thres;
|
||||
cv::absdiff(prev_grey,cur_grey,thres);
|
||||
|
||||
cv::Mat element = getStructuringElement( cv::MORPH_ELLIPSE,
|
||||
cv::Size( 2*ERODE_SIZE + 1, 2*ERODE_SIZE+1 ),
|
||||
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;
|
||||
thres.copyTo(temp);
|
||||
//these two vectors needed for output of findContours
|
||||
vector< vector<Point> > contours;
|
||||
vector<Vec4i> hierarchy;
|
||||
cv::Mat temp;
|
||||
closed_thres.copyTo(temp);
|
||||
vector< vector<cv::Point> > contours;
|
||||
//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
|
||||
cv::findContours(temp,contours,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;
|
||||
else objectDetected = false;
|
||||
if(contours.size()>0){
|
||||
vector<cv::Rect> nc_rects; // Non connected rectangles
|
||||
for(size_t i=0; i<contours.size();i++)
|
||||
{
|
||||
nc_rects.push_back(cv::boundingRect(contours[i]));
|
||||
}
|
||||
|
||||
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<cv::Rect> c_rects; // Connected rectangles
|
||||
cleanBBoxes(nc_rects, cur_grey.size(), c_rects);
|
||||
if (c_rects.size() > 0) {
|
||||
for (const auto& rect : c_rects)
|
||||
cv::rectangle(output, rect, cv::Scalar(0, 255, 0), 2);
|
||||
|
||||
cv::Rect objBRect = c_rects.front();
|
||||
papillon::BoundingBox bbox = papillon::BoundingBox();
|
||||
bbox.x = objBRect.x / (float)cur_grey.size().width;
|
||||
bbox.y = objBRect.y / (float)cur_grey.size().height;
|
||||
bbox.width = objBRect.width / (float)cur_grey.size().width;
|
||||
bbox.height = objBRect.height / (float)cur_grey.size().height;
|
||||
pub_cmd.publish(bbox);
|
||||
}
|
||||
}
|
||||
//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)
|
||||
{
|
||||
twist.angular.z = 0.2;
|
||||
}
|
||||
else if(centre_rect.x > centre_image.x)
|
||||
{
|
||||
twist.angular.z = -0.2;
|
||||
}
|
||||
|
||||
pub_cmd.publish(twist);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
|
|
@ -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;
|
||||
}
|
Loading…
Reference in a new issue