Compare commits

...

7 commits

Author SHA1 Message Date
lhark
1cca8e6411 Commands cheatsheet 2016-06-07 12:56:57 +02:00
lhark
40f52fbf03 Fix recursive stabilisation 2016-06-07 12:56:04 +02:00
lhark
90f55e89cd Optical flow tracking and recursive stabilisation on the way 2016-06-03 01:09:18 +02:00
lhark
a488dbeec2 Warp point vector helper 2016-06-02 18:05:29 +02:00
lhark
a259a853e4 WIP 2016-06-02 01:20:29 +02:00
samilyjcc
e4ac1b70af add threshold to movement 2016-06-01 17:45:32 +02:00
samilyjcc
ce831fb167 changed detection to opt flow 2016-06-01 17:09:28 +02:00
3 changed files with 151 additions and 125 deletions

View file

@ -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 ${OpenCV_INCLUDE_DIRS})
set_property (TARGET papillon APPEND PROPERTY INCLUDE_DIRECTORIES ${catkin_INCLUDE_DIRS}) set_property (TARGET papillon APPEND PROPERTY INCLUDE_DIRECTORIES ${catkin_INCLUDE_DIRS})
set_property (TARGET papillon APPEND PROPERTY LINK_LIBRARIES ${OpenCV_LIBRARIES}) set_property (TARGET papillon APPEND PROPERTY LINK_LIBRARIES ${OpenCV_LIBRARIES})
set( CMAKE_EXPORT_COMPILE_COMMANDS 1 )
install(TARGETS papillon DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(TARGETS papillon DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

1
commands Normal file
View file

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

View file

@ -3,6 +3,7 @@
#include <cv_bridge/cv_bridge.h> #include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h> #include <sensor_msgs/image_encodings.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
#include <typeinfo>
#include <opencv/cv.h> #include <opencv/cv.h>
@ -13,13 +14,26 @@ using namespace std;
class Traite_image { class Traite_image {
public: public:
const static int SENSITIVITY_VALUE = 30; const static int THRESHOLD_DETECT_SENSITIVITY = 10;
const static int BLUR_SIZE = 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 prev;
Mat last_T;
// Stabilisation transformation matrices
Mat T, last_T;
bool first = true; bool first = true;
// Features vectors
vector <Point2f> prev_ftr, cur_ftr;
// Downsize factor
int resize_f = 1; int resize_f = 1;
int theObject[2] = {0,0}; int theObject[2] = {0,0};
@ -66,11 +80,14 @@ class Traite_image {
} }
Mat next_stab; Mat next_stab;
stabiliseImg(prev, next, next_stab); trackFeatures(prev, next);
Rect myROI(next_stab.size().width/8, next_stab.size().height/8, next_stab.size().width*3/4, next_stab.size().height*3/4); stabiliseImg(next, next_stab);
Mat next_stab_cropped = next_stab(myROI); trackingOptFlow(prev, next_stab, next_stab);
Mat prev_cropped = prev(myROI); Mat next_stab2;
searchForMovement(prev_cropped, next_stab_cropped, output); 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()); pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
@ -83,42 +100,38 @@ class Traite_image {
prev = next.clone(); prev = next.clone();
} }
//int to string helper function
string intToString(int number){
//this function has a number input and string output void trackFeatures(Mat prev, Mat cur) {
std::stringstream ss;
ss << number;
return ss.str();
}
void stabiliseImg(Mat prev, Mat cur, Mat &output){
Mat cur_grey, prev_grey; Mat cur_grey, prev_grey;
cvtColor(cur, cur_grey, COLOR_BGR2GRAY); cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
cvtColor(prev, prev_grey, COLOR_BGR2GRAY); cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
// vector from prev to cur // vector from prev to cur
vector <Point2f> prev_corner, cur_corner; vector <Point2f> prev_corner, cur_corner;
vector <Point2f> prev_corner2, cur_corner2;
vector <uchar> status; vector <uchar> status;
vector <float> err; 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); calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
// weed out bad matches // weed out bad matches
prev_ftr.resize(0);
cur_ftr.resize(0);
for(size_t i=0; i < status.size(); i++) { for(size_t i=0; i < status.size(); i++) {
if(status[i]) { if(status[i]) {
prev_corner2.push_back(prev_corner[i]); prev_ftr.push_back(prev_corner[i]);
cur_corner2.push_back(cur_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); last_T.copyTo(T);
} else
T.copyTo(last_T); T.copyTo(last_T);
Mat cur2; Mat cur2;
@ -128,58 +141,59 @@ class Traite_image {
cur2.copyTo(output); 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); cur.copyTo(output);
cvtColor(prev, prev_grey, COLOR_BGR2GRAY); vector <Point2f> cur_ftr_stab;
cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
// Subtract the 2 last frames and threshold them //T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
Mat thres; //if(T.data == NULL)
absdiff(prev_grey,cur_grey,thres); // last_T.copyTo(T);
threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY); //else
// Blur to eliminate noise // T.copyTo(last_T);
blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE));
threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
//notice how we use the '&' operator for objectDetected and output. This is because we wish warpPoints(cur_ftr, cur_ftr_stab, T, true);
//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
//if contours vector is not empty, we have found some objects vector <Point2f> objects;
if(contours.size()>0)objectDetected=true; vector <float> flow_norm;
else objectDetected = false; 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);
if(objectDetected){ line(output, prev_ftr[i], cur_ftr[i], Scalar(200,0,0),1);
//the largest contour is found at the end of the contours vector line(output, prev_ftr[i], cur_ftr_stab[i], Scalar(0,200,0),1);
//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 for(size_t i=0; i < flow_norm.size(); ++i) {
rectangle(output, Point(x,y), Point(x+width, y+height), Scalar(0, 255, 0), 2); if(flow_norm[i] > MOVEMENT_THRES) {
objects.push_back(cur_ftr_stab[i]);
//write the position of the object to the screen prev_ftr.erase(prev_ftr.begin() + i);
putText(output,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2); 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) inline bool isFlowCorrect(Point2f u)
{ {
@ -194,17 +208,27 @@ class Traite_image {
geometry_msgs::Twist twist = geometry_msgs::Twist(); 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; 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; twist.angular.z = -0.2;
} }
pub_cmd.publish(twist); 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();
}
}; };