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

Add background movement compensation

Browse source
This commit is contained in:
lhark 2016-05-18 00:46:44 +02:00
parent 3830c1e411
commit 3dbe6d1925
1 changed files with 80 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

89
src/papillon.cpp
View file

@ -16,8 +16,9 @@ class Traite_image {
const static int BLUR_SIZE = 10;
Mat prev;
Mat last_T;
bool first = true;
int resize_f = 1;
int resize_f = 4;
int theObject[2] = {0,0};
Rect objectBoundingRectangle = Rect(0,0,0,0);
@ -51,9 +52,10 @@ class Traite_image {
//Mat& input = const_cast<Mat&>(bridge_input->image);
const Mat& input = bridge_input->image;
Mat next;
Mat next_grey;
resize(input, next, Size(input.size().width/resize_f, input.size().height/resize_f));
cvtColor(next, next, CV_BGR2GRAY);
Mat output = input.clone(); // (input.rows, input.cols, CV_32FC2);
cvtColor(next, next_grey, CV_BGR2GRAY);
Mat output;// = input.clone(); // (input.rows, input.cols, CV_32FC2);
//ROS_INFO("got input");
if (first) {
prev = next.clone();
@ -61,14 +63,16 @@ class Traite_image {
ROS_INFO("first done");
}
stabiliseImg(prev, next, output);
// Subtract the 2 last frames and threshold them
Mat thres;
absdiff(prev,next,thres);
threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
//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, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
searchForMovement(thres, output);
//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.
@ -87,6 +91,73 @@ class Traite_image {
return ss.str();
}
void stabiliseImg(Mat prev, Mat cur, Mat &output){
Mat prev_grey, cur_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;
vector <Point2f> prev_corner2, cur_corner2;
vector <uchar> status;
vector <float> err;
goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30);
calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
// weed out bad matches
for(size_t i=0; i < status.size(); i++) {
if(status[i]) {
prev_corner2.push_back(prev_corner[i]);
cur_corner2.push_back(cur_corner[i]);
}
}
// translation + rotation only
Mat T(2, 3, CV_32FC1);
T = estimateRigidTransform(prev_corner2, cur_corner2, false); // false = rigid transform, no scaling/shearing
// 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;
// in rare cases no transform is found. We'll just use the last known good transform.
if(T.data == NULL) {
last_T.copyTo(T);
}
T.copyTo(last_T);
/// 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){
//notice how we use the '&' operator for objectDetected and cameraFeed. This is because we wish
//to take the values passed into the function and manipulate them, rather than just working with a copy.