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

Implement Kyle Hounslow's greyscale threshold motion tracking example

Browse source
This commit is contained in:
lhark 2016-05-16 00:27:55 +02:00
parent 14cdb2c0fb
commit 3830c1e411
1 changed files with 74 additions and 23 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

97
src/papillon.cpp
View file

@ -12,9 +12,15 @@ using namespace std;
class Traite_image { class Traite_image {
public: public:
const static int SENSITIVITY_VALUE = 30;
const static int BLUR_SIZE = 10;
Mat prev; Mat prev;
bool first = true; bool first = true;
int resize_f = 8; int resize_f = 1;
int theObject[2] = {0,0};
Rect objectBoundingRectangle = Rect(0,0,0,0);
ros::NodeHandle n; ros::NodeHandle n;
@ -47,42 +53,87 @@ class Traite_image {
Mat next; Mat next;
resize(input, next, Size(input.size().width/resize_f, input.size().height/resize_f)); resize(input, next, Size(input.size().width/resize_f, input.size().height/resize_f));
cvtColor(next, next, CV_BGR2GRAY); cvtColor(next, next, CV_BGR2GRAY);
Mat output; // (input.rows, input.cols, CV_32FC2); Mat output = input.clone(); // (input.rows, input.cols, CV_32FC2);
ROS_INFO("got input"); //ROS_INFO("got input");
if (first) { if (first) {
prev = next.clone(); prev = next.clone();
first = false; first = false;
ROS_INFO("first done"); ROS_INFO("first done");
} }
//unsigned int size = input.rows * input.cols * 3; // Subtract the 2 last frames and threshold them
//unsigned char* begin_input = (unsigned char*)(input.data); Mat thres;
//unsigned char* end_input = (unsigned char*)(input.data) + size; absdiff(prev,next,thres);
//unsigned char* out = (unsigned char*)(output.data); threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
//unsigned char* in = begin_input; // Blur to eliminate noise
blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE));
// This is an efficient way to process each channel in each pixel, threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
// with an iterator taste. searchForMovement(thres, output);
//while(in != end_input) {
// *(out++) = *(ptr_prev) - *(in);
// *(ptr_prev++) = *(in++);
//}
Mat_<Point2f> flow;
Ptr<DenseOpticalFlow> tvl1 = createOptFlow_DualTVL1();
tvl1->calc(prev, next, flow);
drawOpticalFlow(flow, 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.
ROS_INFO("pub"); //ROS_INFO("pub");
prev = next.clone(); prev = next.clone();
} }
//int to string helper function
string intToString(int number){
//this function has a number input and string output
std::stringstream ss;
ss << number;
return ss.str();
}
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.
//eg. we draw to the cameraFeed to be displayed in the main() function.
bool objectDetected = false;
Mat temp;
thresholdImage.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));
int xpos = objectBoundingRectangle.x+objectBoundingRectangle.width/2;
int ypos = objectBoundingRectangle.y+objectBoundingRectangle.height/2;
//update the objects positions by changing the 'theObject' array values
theObject[0] = xpos , theObject[1] = ypos;
}
//make some temp x and y variables so we dont have to type out so much
int x = theObject[0];
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);
//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);
}
inline bool isFlowCorrect(Point2f u) inline bool isFlowCorrect(Point2f u)
{ {
return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9; return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;