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

remove namespace cv

Browse source
This commit is contained in:
samilyjcc 2016-06-14 11:56:34 +02:00
parent ca53673111
commit 0e85119c64
1 changed files with 57 additions and 78 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

135
src/papillon.cpp
View file

@ -8,28 +8,27 @@
#include <sstream> #include <sstream>
using namespace cv;
using namespace std; using namespace std;
class Traite_image { class Traite_image {
public: public:
const static int SENSITIVITY_VALUE = 40; const static int SENSITIVITY_VALUE = 40;
const static int BLUR_SIZE = 15; const static int BLUR_Size = 15;
Mat prev; cv::Mat prev;
Mat last_T; cv::Mat last_T;
bool first = true; bool first = true;
int resize_f = 2; int resize_f = 2;
int theObject[2] = {0,0}; int theObject[2] = {0,0};
Rect objectBoundingRectangle = Rect(0,0,0,0); cv::Rect objectBoundingRectangle = cv::Rect(0,0,0,0);
ros::NodeHandle n; ros::NodeHandle n;
image_transport::ImageTransport it; image_transport::ImageTransport it;
image_transport::Publisher pub_img; image_transport::Publisher pub_img;
ros::Publisher pub_cmd; ros::Publisher pub_cmd;
image_transport::Subscriber sub; image_transport::Subscriber sub;
@ -47,17 +46,17 @@ class Traite_image {
try { try {
bridge_input = cv_bridge::toCvShare(msg,sensor_msgs::image_encodings::RGB8); bridge_input = cv_bridge::toCvShare(msg,sensor_msgs::image_encodings::RGB8);
} }
catch (Exception& e) { catch (cv::Exception& e) {
std::ostringstream errstr; std::ostringstream errstr;
errstr << "cv_bridge exception caught: " << e.what(); errstr << "cv_bridge exception caught: " << e.what();
return; return;
} }
//Mat& input = const_cast<Mat&>(bridge_input->image); //cv::Mat& input = const_cast<cv::Mat&>(bridge_input->image);
const Mat& input = bridge_input->image; const cv::Mat& input = bridge_input->image;
Mat next; cv::Mat next;
resize(input, next, Size(input.size().width/resize_f, input.size().height/resize_f)); resize(input, next, cv::Size(input.size().width/resize_f, input.size().height/resize_f));
Mat output;// = input.clone(); // (input.rows, input.cols, CV_32FC2); cv::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();
@ -65,16 +64,16 @@ class Traite_image {
ROS_INFO("first done"); ROS_INFO("first done");
} }
Mat next_stab; cv::Mat next_stab;
stabiliseImg(prev, next, next_stab); stabiliseImg(prev, next, next_stab);
int crop_ratio = 6; int crop_ratio = 6;
float crop_x = next_stab.size().width/crop_ratio; float crop_x = next_stab.size().width/crop_ratio;
float crop_y = next_stab.size().height/crop_ratio; float crop_y = next_stab.size().height/crop_ratio;
float crop_w = next_stab.size().width*(1-2.0/crop_ratio); float crop_w = next_stab.size().width*(1-2.0/crop_ratio);
float crop_h = next_stab.size().height*(1-2.0/crop_ratio); float crop_h = next_stab.size().height*(1-2.0/crop_ratio);
Rect myROI(crop_x, crop_y, crop_w, crop_h); cv::Rect myROI(crop_x, crop_y, crop_w, crop_h);
Mat next_stab_cropped = next_stab(myROI); cv::Mat next_stab_cropped = next_stab(myROI);
Mat prev_cropped = prev(myROI); cv::Mat prev_cropped = prev(myROI);
searchForMovement(prev_cropped, next_stab_cropped, output); searchForMovement(prev_cropped, next_stab_cropped, output);
@ -97,21 +96,21 @@ class Traite_image {
return ss.str(); return ss.str();
} }
void stabiliseImg(Mat prev, Mat cur, Mat &output){ void stabiliseImg(cv::Mat prev, cv::Mat cur, cv::Mat &output){
Mat cur_grey, prev_grey; cv::Mat cur_grey, prev_grey;
cvtColor(cur, cur_grey, COLOR_BGR2GRAY); cv::cvtColor(cur, cur_grey, cv::COLOR_BGR2GRAY);
cvtColor(prev, prev_grey, COLOR_BGR2GRAY); cv::cvtColor(prev, prev_grey, cv::COLOR_BGR2GRAY);
// vector from prev to cur // vector from prev to cur
vector <Point2f> prev_corner, cur_corner; vector <cv::Point2f> prev_corner, cur_corner;
vector <Point2f> prev_corner2, cur_corner2; vector <cv::Point2f> prev_corner2, cur_corner2;
vector <uchar> status; vector <uchar> status;
vector <float> err; vector <float> err;
goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30); cv::goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30);
calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err); cv::calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
// weed out bad matches // weed out bad cv::Matches
for(size_t i=0; i < status.size(); i++) { for(size_t i=0; i < status.size(); i++) {
if(status[i]) { if(status[i]) {
prev_corner2.push_back(prev_corner[i]); prev_corner2.push_back(prev_corner[i]);
@ -119,51 +118,51 @@ class Traite_image {
} }
} }
Mat T = estimateRigidTransform(prev_corner2, cur_corner2, true); // false = rigid transform, no scaling/shearing cv::Mat T = estimateRigidTransform(prev_corner2, cur_corner2, true); // false = rigid transform, no scaling/shearing
if(T.data == NULL) { if(T.data == NULL) {
last_T.copyTo(T); last_T.copyTo(T);
} }
T.copyTo(last_T); T.copyTo(last_T);
Mat cur2; cv::Mat cur2;
warpAffine(cur, cur2, T, cur.size(),INTER_CUBIC|WARP_INVERSE_MAP); cv::warpAffine(cur, cur2, T, cur.size(),cv::INTER_CUBIC|cv::WARP_INVERSE_MAP);
cur2.copyTo(output); cur2.copyTo(output);
} }
void searchForMovement(Mat prev, Mat cur, Mat &output){ void searchForMovement(cv::Mat prev, cv::Mat cur, cv::Mat &output){
Mat cur_grey, prev_grey; cv::Mat cur_grey, prev_grey;
cur.copyTo(output); cur.copyTo(output);
cvtColor(prev, prev_grey, COLOR_BGR2GRAY); cv::cvtColor(prev, prev_grey, cv::COLOR_BGR2GRAY);
cvtColor(cur, cur_grey, COLOR_BGR2GRAY); cv::cvtColor(cur, cur_grey, cv::COLOR_BGR2GRAY);
GaussianBlur(prev_grey, prev_grey, Size(BLUR_SIZE,BLUR_SIZE), 3.0); cv::GaussianBlur(prev_grey, prev_grey, cv::Size(BLUR_Size,BLUR_Size), 3.0);
GaussianBlur(cur_grey, cur_grey, Size(BLUR_SIZE,BLUR_SIZE), 3.0); cv::GaussianBlur(cur_grey, cur_grey, cv::Size(BLUR_Size,BLUR_Size), 3.0);
//blur(prev_grey, prev_grey, Size(BLUR_SIZE, BLUR_SIZE)); //blur(prev_grey, prev_grey, cv::Size(BLUR_Size, BLUR_Size));
//blur(cur_grey, cur_grey, Size(BLUR_SIZE, BLUR_SIZE)); //blur(cur_grey, cur_grey, cv::Size(BLUR_Size, BLUR_Size));
// Subtract the 2 last frames and threshold them // Subtract the 2 last frames and threshold them
Mat thres; cv::Mat thres;
absdiff(prev_grey,cur_grey,thres); cv::absdiff(prev_grey,cur_grey,thres);
// threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY); // threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY);
// // Blur to eliminate noise // // Blur to eliminate noise
// blur(thres, thres, Size(BLUR_SIZE, BLUR_SIZE)); // blur(thres, thres, cv::Size(BLUR_Size, BLUR_Size));
threshold(thres, thres, SENSITIVITY_VALUE, 255, THRESH_BINARY); cv::threshold(thres, thres, SENSITIVITY_VALUE, 255, cv::THRESH_BINARY);
//~ int dilation_size = 2; //~ int dilation_Size = 2;
//~ Mat element = getStructuringElement( MORPH_ELLIPSE, //~ cv::Mat element = getStructuringElement( MORPH_ELLIPSE,
//~ Size( 2*dilation_size + 1, 2*dilation_size+1 ), //~ cv::Size( 2*dilation_Size + 1, 2*dilation_Size+1 ),
//~ Point( dilation_size, dilation_size ) ); //~ Point( dilation_Size, dilation_Size ) );
//~ // Apply the dilation operation //~ // Apply the dilation operation
//~ Mat dilated_thres; //~ cv::Mat dilated_thres;
//~ dilate(thres, dilated_thres, element ); //~ dilate(thres, dilated_thres, element );
//~ //~
//~ dilated_thres.copyTo(output); //~ dilated_thres.copyTo(output);
Mat closed_thres; cv::Mat closed_thres;
Mat structuringElement = getStructuringElement(MORPH_ELLIPSE, Size(40, 40)); cv::Mat structuringElement = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(40, 40));
morphologyEx( thres, closed_thres, MORPH_CLOSE, structuringElement ); cv::morphologyEx( thres, closed_thres, cv::MORPH_CLOSE, structuringElement );
//closed_thres.copyTo(output); //closed_thres.copyTo(output);
@ -171,14 +170,14 @@ class Traite_image {
//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 output 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; cv::Mat temp;
closed_thres.copyTo(temp); closed_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<cv::Point> > contours;
vector<Vec4i> hierarchy; vector<cv::Vec4i> hierarchy;
//find contours of filtered image using openCV findContours function //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_CCOMP,CV_CHAIN_APPROX_SIMPLE );// retrieves all contours
findContours(temp,contours,hierarchy,CV_RETR_EXTERNAL,CV_CHAIN_APPROX_SIMPLE );// retrieves external contours cv::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 vector is not empty, we have found some objects
if(contours.size()>0)objectDetected=true; if(contours.size()>0)objectDetected=true;
@ -190,11 +189,11 @@ class Traite_image {
//vector< vector<Point> > largestContourVec; //vector< vector<Point> > largestContourVec;
//largestContourVec.push_back(contours.at(contours.size()-1)); //largestContourVec.push_back(contours.at(contours.size()-1));
//make a bounding rectangle around the largest contour then find its centroid //make a bounding rectangle around the largest contour then find its centroid
//this will be the object's final estimated position. //this will be the object's final esticv::Mated position.
for(int i=0; i<contours.size();i++) for(int i=0; i<contours.size();i++)
{ {
objectBoundingRectangle = boundingRect(contours[i]); objectBoundingRectangle = cv::boundingRect(contours[i]);
rectangle(output, objectBoundingRectangle, Scalar(0, 255, 0), 2); cv::rectangle(output, objectBoundingRectangle, cv::Scalar(0, 255, 0), 2);
} }
} }
//make some temp x and y variables so we dont have to type out so much //make some temp x and y variables so we dont have to type out so much
@ -204,36 +203,16 @@ class Traite_image {
//~ int height = objectBoundingRectangle.height; //~ int height = objectBoundingRectangle.height;
//draw a rectangle around the object //draw a rectangle around the object
//rectangle(output, Point(x,y), Point(x+width, y+height), Scalar(0, 255, 0), 2); //rectangle(output, Point(x,y), Point(x+width, y+height), cv::Scalar(0, 255, 0), 2);
//write the position of the object to the screen //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); //putText(output,"Tracking object at (" + intToString(x)+","+intToString(y)+")",Point(x,y),1,1,cv::Scalar(255,0,0),2);
} }
inline bool isFlowCorrect(Point2f u) inline bool isFlowCorrect(cv::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;
} }
void droneTracking(Rect img_size)
{
Point2f centre_image = Point2f(img_size.width/2, img_size.height/2);
Point2f centre_rect = Point2f(objectBoundingRectangle.x + objectBoundingRectangle.width/2, objectBoundingRectangle.y + objectBoundingRectangle.height/2);
geometry_msgs::Twist twist = geometry_msgs::Twist();
if(centre_rect.x < centre_image.x)
{
twist.angular.z = 0.2;
}
else if(centre_rect.x > centre_image.x)
{
twist.angular.z = -0.2;
}
pub_cmd.publish(twist);
}
}; };