lhark
/
papillon
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

Optical flow tracking and recursive stabilisation on the way

optimise-flow
lhark 10 years ago
parent
a488dbeec2
commit
90f55e89cd
1 changed files with 35 additions and 128 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 163
      src/papillon.cpp

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) {

Write Preview
Loading…
Cancel
Save