lhark/papillon
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Optical flow tracking and recursive stabilisation on the way

Browse source
This commit is contained in:
lhark 2016-06-03 01:09:18 +02:00
parent a488dbeec2
commit 90f55e89cd
1 changed files with 35 additions and 128 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

163
src/papillon.cpp
View file

@ -17,18 +17,24 @@ class Traite_image {
const static int THRESHOLD_DETECT_SENSITIVITY = 10;
const static int BLUR_SIZE = 5;
const static int THRESHOLD_MOV = 5;
const static int MOVEMENT_THRES = 0.1;
constexpr static float MOVEMENT_THRES = 0.1;
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 = 2;
int resize_f = 1;
int theObject[2] = {0,0};
Rect objectBoundingRectangle = Rect(0,0,0,0);
@ -75,10 +81,10 @@ 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);
trackingOptFlow(prev_cropped, next_stab_cropped, output);
trackingOptFlow(prev, next_stab, next_stab);
Mat next_stab2;
stabiliseImg(prev, next, next_stab2);
trackingOptFlow(prev, next_stab2, output);
//searchForMovementOptFlow(prev_cropped, next_stab_cropped, output);
@ -112,7 +118,7 @@ class Traite_image {
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
@ -125,12 +131,12 @@ class Traite_image {
}
}
Mat T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
if(T.data == NULL) {
if(T.data == NULL)
last_T.copyTo(T);
}
T.copyTo(last_T);
else
T.copyTo(last_T);
Mat cur2;
@ -139,115 +145,6 @@ class Traite_image {
cur2.copyTo(output);
}
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);
// Subtract the 2 last frames and threshold them
Mat thres;
absdiff(prev_grey,cur_grey,thres);
threshold(thres, thres, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY);
// Blur to eliminate noise
blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE));
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.
//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
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));
}
//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){
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);
bool objectDetected = false;
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 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));
}
//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 warpPoints(vector<Point2f> p, vector<Point2f> &p_warp, Mat T, bool invert=false) {
if(invert)
invertAffineTransform(T, T);
@ -266,17 +163,27 @@ class Traite_image {
}
void trackingOptFlow(Mat prev, Mat cur, Mat &output) {
vector <Point2f> curc_stab;
cur.copyTo(output);
vector <Point2f> cur_ftr_stab;
Mat T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
ROS_INFO("ready to warp");
warpPoints(cur_ftr, curc_stab, T, true);
ROS_INFO("warped");
if(T.data == NULL)
last_T.copyTo(T);
else
T.copyTo(last_T);
warpPoints(cur_ftr, cur_ftr_stab, T, true);
vector <Point2f> objects;
for(size_t i=0; i < prev_ftr.size(); ++i) {
float flow_norm = norm(prev_ftr[i] - cur_ftr[i]) / prev.size().height;
if(flow_norm > MOVEMENT_THRES)
objects.push_back(cur_ftr[i]);
float flow_norm = 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);
if(flow_norm > 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) {