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

Threshold motion detection not working well

Browse source
This commit is contained in:
lhark 2016-05-24 11:57:41 +02:00
parent 8bd67f3203
commit cb55c6dcd3
1 changed files with 36 additions and 93 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

129
src/papillon.cpp
View file

@ -20,7 +20,7 @@ class Traite_image {
Mat prev; Mat prev;
Mat last_T; Mat last_T;
bool first = true; bool first = true;
int resize_f = 4; int resize_f = 1;
int theObject[2] = {0,0}; int theObject[2] = {0,0};
Rect objectBoundingRectangle = Rect(0,0,0,0); Rect objectBoundingRectangle = Rect(0,0,0,0);
@ -63,16 +63,10 @@ class Traite_image {
ROS_INFO("first done"); ROS_INFO("first done");
} }
stabiliseImg(prev, next, output); Mat next_stab;
stabiliseImg(prev, next, next_stab);
searchForMovement(prev, next_stab, output);
// Subtract the 2 last frames and threshold them
//Mat thres;
//absdiff(prev,next,thres);
//threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
// Blur to eliminate noise
//blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE));
//threshold(thres, output, SENSITIVITY_VALUE, 255, THRESH_BINARY);
//searchForMovement(thres, output);
pub.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg()); pub.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
// bridge_input is handled by a smart-pointer. No explicit delete needed. // bridge_input is handled by a smart-pointer. No explicit delete needed.
@ -92,12 +86,9 @@ class Traite_image {
} }
void stabiliseImg(Mat prev, Mat cur, Mat &output){ void stabiliseImg(Mat prev, Mat cur, Mat &output){
Mat prev_grey, cur_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);
//Point2f srcTri[3];
//Point2f dstTri[3];
Mat warp_mat( 2, 3, CV_32FC1 );
// vector from prev to cur // vector from prev to cur
vector <Point2f> prev_corner, cur_corner; vector <Point2f> prev_corner, cur_corner;
@ -116,88 +107,40 @@ class Traite_image {
} }
} }
// translation + rotation only Mat T = estimateRigidTransform(prev_corner2, cur_corner2, true); // false = rigid transform, no scaling/shearing
// Mat T(2, 3, CV_32FC1);
Mat T = estimateRigidTransform(prev_corner2, cur_corner2, false); // false = rigid transform, no scaling/shearing
ROS_INFO("coucou1"); if(T.data == NULL) {
// in rare cases no transform is found. We'll just use the last known good transform. last_T.copyTo(T);
/*if(T.data == NULL) { }
last_T = T.clone(); T.copyTo(last_T);
}
T = last_T.clone();*/ Mat cur2;
ROS_INFO("coucou2");
// 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));
// cv::Mat H = cv::Mat(3,3,T.type());
// H.at<double>(0,0) = T.at<double>(0,0);
// H.at<double>(0,1) = T.at<double>(0,1);
// H.at<double>(0,2) = T.at<double>(0,2);
// H.at<double>(1,0) = T.at<double>(1,0); warpAffine(cur, cur2, T, cur.size(),INTER_NEAREST|WARP_INVERSE_MAP);
// H.at<double>(1,1) = T.at<double>(1,1);
// H.at<double>(1,2) = T.at<double>(1,2);
// H.at<double>(2,0) = 0.0; cur2.copyTo(output);
// H.at<double>(2,1) = 0.0;
// H.at<double>(2,2) = 1.0;
Mat T1(2, 3, CV_32FC1);
T1.at<double>(0,0) = cos(da);
T1.at<double>(0,1) = -sin(da);
T1.at<double>(1,0) = sin(da);
T1.at<double>(1,1) = cos(da);
T1.at<double>(0,2) = dx;
T1.at<double>(1,2) = dy;
Mat cur2;
warpAffine(cur, cur2, T1, cur.size());
int vert_border = HORIZONTAL_BORDER_CROP * prev.rows / prev.cols; // get the aspect ratio correct
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());
output = cur2.clone();
/*
/// Set your 3 points to calculate the Affine Transform
srcTri[0] = Point2f( 0,0 );
srcTri[1] = Point2f( prev.cols, 0 );
srcTri[2] = Point2f( 0, prev.rows );
dstTri[0] = Point2f( prev.cols / 2, prev.rows / 2 );
dstTri[1] = Point2f( prev.cols * 3 / 2, prev.rows / 2);
dstTri[2] = Point2f( prev.cols / 2, prev.rows * 3 / 2 );
/// Get the Affine Transform
warp_mat = getAffineTransform( srcTri, dstTri );
warpAffine(prev,output,T,cv::Size(prev.cols+cur.cols,prev.rows+cur.rows));
warpAffine(output,output,warp_mat,cv::Size(prev.cols+cur.cols,prev.rows+cur.rows));
warpAffine(cur,cur,warp_mat,cv::Size(prev.cols+cur.cols,prev.rows+cur.rows));
//Mat half(output, cv::Rect(0, 0,cur.cols,cur.rows));
cur.copyTo(output, cur);
*
*/
} }
void searchForMovement(Mat thresholdImage, Mat &cameraFeed){ void searchForMovement(Mat prev, Mat cur, Mat &output){
//notice how we use the '&' operator for objectDetected and cameraFeed. This is because we wish 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, 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);
//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. //to take the values passed into the function and manipulate them, rather than just working with a copy.
//eg. we draw to the cameraFeed to be displayed in the main() function. //eg. we draw to the output to be displayed in the main() function.
bool objectDetected = false; bool objectDetected = false;
Mat temp; Mat temp;
thresholdImage.copyTo(temp); thres.copyTo(temp);
//these two vectors needed for output of findContours //these two vectors needed for output of findContours
vector< vector<Point> > contours; vector< vector<Point> > contours;
vector<Vec4i> hierarchy; vector<Vec4i> hierarchy;
@ -228,14 +171,14 @@ class Traite_image {
int y = theObject[1]; int y = theObject[1];
//draw some crosshairs around the object //draw some crosshairs around the object
circle(cameraFeed,Point(x,y),20,Scalar(0,255,0),2); circle(output,Point(x,y),20,Scalar(0,255,0),2);
line(cameraFeed,Point(x,y),Point(x,y-25),Scalar(0,255,0),2); line(output,Point(x,y),Point(x,y-25),Scalar(0,255,0),2);
line(cameraFeed,Point(x,y),Point(x,y+25),Scalar(0,255,0),2); line(output,Point(x,y),Point(x,y+25),Scalar(0,255,0),2);
line(cameraFeed,Point(x,y),Point(x-25,y),Scalar(0,255,0),2); line(output,Point(x,y),Point(x-25,y),Scalar(0,255,0),2);
line(cameraFeed,Point(x,y),Point(x+25,y),Scalar(0,255,0),2); line(output,Point(x,y),Point(x+25,y),Scalar(0,255,0),2);
//write the position of the object to the screen //write the position of the object to the screen
putText(cameraFeed,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2); putText(output,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,Scalar(255,0,0),2);
} }
inline bool isFlowCorrect(Point2f u) inline bool isFlowCorrect(Point2f u)