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

Threshold motion detection not working well

drone_control
lhark 10 years ago
parent
8bd67f3203
commit
cb55c6dcd3
1 changed files with 40 additions and 97 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. 123
      src/papillon.cpp

123
src/papillon.cpp
View File

@ -20,7 +20,7 @@ class Traite_image {
Mat prev;
Mat last_T;
bool first = true;
int resize_f = 4;
int resize_f = 1;
int theObject[2] = {0,0};
Rect objectBoundingRectangle = Rect(0,0,0,0);
@ -63,16 +63,10 @@ class Traite_image {
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());
// 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){
Mat prev_grey, cur_grey;
Mat cur_grey, prev_grey;
cvtColor(cur, cur_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 <Point2f> prev_corner, cur_corner;
@ -116,88 +107,40 @@ class Traite_image {
}
}
// translation + rotation only
// Mat T(2, 3, CV_32FC1);
Mat T = estimateRigidTransform(prev_corner2, cur_corner2, false); // false = rigid transform, no scaling/shearing
Mat T = estimateRigidTransform(prev_corner2, cur_corner2, true); // false = rigid transform, no scaling/shearing
ROS_INFO("coucou1");
// in rare cases no transform is found. We'll just use the last known good transform.
/*if(T.data == NULL) {
last_T = T.clone();
if(T.data == NULL) {
last_T.copyTo(T);
}
T = last_T.clone();*/
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);
// 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;
// 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;
T.copyTo(last_T);
Mat cur2;
warpAffine(cur, cur2, T1, cur.size());
int vert_border = HORIZONTAL_BORDER_CROP * prev.rows / prev.cols; // get the aspect ratio correct
warpAffine(cur, cur2, T, cur.size(),INTER_NEAREST|WARP_INVERSE_MAP);
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 );
cur2.copyTo(output);
}
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 );
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);
/// 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);
*
*/
}
// 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);
void searchForMovement(Mat thresholdImage, Mat &cameraFeed){
//notice how we use the '&' operator for objectDetected and cameraFeed. This is because we wish
//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 cameraFeed to be displayed in the main() function.
//eg. we draw to the output to be displayed in the main() function.
bool objectDetected = false;
Mat temp;
thresholdImage.copyTo(temp);
thres.copyTo(temp);
//these two vectors needed for output of findContours
vector< vector<Point> > contours;
vector<Vec4i> hierarchy;
@ -228,14 +171,14 @@ class Traite_image {
int y = theObject[1];
//draw some crosshairs around the object
circle(cameraFeed,Point(x,y),20,Scalar(0,255,0),2);
line(cameraFeed,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(cameraFeed,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);
circle(output,Point(x,y),20,Scalar(0,255,0),2);
line(output,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(output,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
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)

Write Preview
Loading…
Cancel
Save