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>
using namespace cv;
using namespace std;
class Traite_image {
public:
const static int SENSITIVITY_VALUE = 40;
const static int BLUR_SIZE = 15;
const static int BLUR_Size = 15;
Mat prev;
Mat last_T;
cv::Mat prev;
cv::Mat last_T;
bool first = true;
int resize_f = 2;
int theObject[2] = {0,0};
Rect objectBoundingRectangle = Rect(0,0,0,0);
cv::Rect objectBoundingRectangle = cv::Rect(0,0,0,0);
ros::NodeHandle n;
image_transport::ImageTransport it;
image_transport::Publisher pub_img;
ros::Publisher pub_cmd;
ros::Publisher pub_cmd;
image_transport::Subscriber sub;
@ -47,17 +46,17 @@ class Traite_image {
try {
bridge_input = cv_bridge::toCvShare(msg,sensor_msgs::image_encodings::RGB8);
}
catch (Exception& e) {
catch (cv::Exception& e) {
std::ostringstream errstr;
errstr << "cv_bridge exception caught: " << e.what();
return;
}
//Mat& input = const_cast<Mat&>(bridge_input->image);
const Mat& input = bridge_input->image;
Mat next;
resize(input, next, Size(input.size().width/resize_f, input.size().height/resize_f));
Mat output;// = input.clone(); // (input.rows, input.cols, CV_32FC2);
//cv::Mat& input = const_cast<cv::Mat&>(bridge_input->image);
const cv::Mat& input = bridge_input->image;
cv::Mat next;
resize(input, next, cv::Size(input.size().width/resize_f, input.size().height/resize_f));
cv::Mat output;// = input.clone(); // (input.rows, input.cols, CV_32FC2);
//ROS_INFO("got input");
if (first) {
prev = next.clone();
@ -65,16 +64,16 @@ class Traite_image {
ROS_INFO("first done");
}
Mat next_stab;
cv::Mat next_stab;
stabiliseImg(prev, next, next_stab);
int crop_ratio = 6;
float crop_x = next_stab.size().width/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_h = next_stab.size().height*(1-2.0/crop_ratio);
Rect myROI(crop_x, crop_y, crop_w, crop_h);
Mat next_stab_cropped = next_stab(myROI);
Mat prev_cropped = prev(myROI);
cv::Rect myROI(crop_x, crop_y, crop_w, crop_h);
cv::Mat next_stab_cropped = next_stab(myROI);
cv::Mat prev_cropped = prev(myROI);
searchForMovement(prev_cropped, next_stab_cropped, output);
@ -97,21 +96,21 @@ class Traite_image {
return ss.str();
}
void stabiliseImg(Mat prev, Mat cur, Mat &output){
Mat cur_grey, prev_grey;
cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
void stabiliseImg(cv::Mat prev, cv::Mat cur, cv::Mat &output){
cv::Mat cur_grey, prev_grey;
cv::cvtColor(cur, cur_grey, cv::COLOR_BGR2GRAY);
cv::cvtColor(prev, prev_grey, cv::COLOR_BGR2GRAY);
// vector from prev to cur
vector <Point2f> prev_corner, cur_corner;
vector <Point2f> prev_corner2, cur_corner2;
vector <cv::Point2f> prev_corner, cur_corner;
vector <cv::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);
cv::goodFeaturesToTrack(prev_grey, prev_corner, 200, 0.01, 30);
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++) {
if(status[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) {
last_T.copyTo(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);
}
void searchForMovement(Mat prev, Mat cur, Mat &output){
Mat cur_grey, prev_grey;
void searchForMovement(cv::Mat prev, cv::Mat cur, cv::Mat &output){
cv::Mat cur_grey, prev_grey;
cur.copyTo(output);
cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
GaussianBlur(prev_grey, prev_grey, Size(BLUR_SIZE,BLUR_SIZE), 3.0);
GaussianBlur(cur_grey, cur_grey, Size(BLUR_SIZE,BLUR_SIZE), 3.0);
//blur(prev_grey, prev_grey, Size(BLUR_SIZE, BLUR_SIZE));
//blur(cur_grey, cur_grey, Size(BLUR_SIZE, BLUR_SIZE));
cv::cvtColor(prev, prev_grey, cv::COLOR_BGR2GRAY);
cv::cvtColor(cur, cur_grey, cv::COLOR_BGR2GRAY);
cv::GaussianBlur(prev_grey, prev_grey, cv::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, cv::Size(BLUR_Size, BLUR_Size));
//blur(cur_grey, cur_grey, cv::Size(BLUR_Size, BLUR_Size));
// Subtract the 2 last frames and threshold them
Mat thres;
absdiff(prev_grey,cur_grey,thres);
cv::Mat thres;
cv::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);
// blur(thres, thres, cv::Size(BLUR_Size, BLUR_Size));
cv::threshold(thres, thres, SENSITIVITY_VALUE, 255, cv::THRESH_BINARY);
//~ int dilation_size = 2;
//~ Mat element = getStructuringElement( MORPH_ELLIPSE,
//~ Size( 2*dilation_size + 1, 2*dilation_size+1 ),
//~ Point( dilation_size, dilation_size ) );
//~ int dilation_Size = 2;
//~ cv::Mat element = getStructuringElement( MORPH_ELLIPSE,
//~ cv::Size( 2*dilation_Size + 1, 2*dilation_Size+1 ),
//~ Point( dilation_Size, dilation_Size ) );
//~ // Apply the dilation operation
//~ Mat dilated_thres;
//~ cv::Mat dilated_thres;
//~ dilate(thres, dilated_thres, element );
//~
//~ dilated_thres.copyTo(output);
Mat closed_thres;
Mat structuringElement = getStructuringElement(MORPH_ELLIPSE, Size(40, 40));
morphologyEx( thres, closed_thres, MORPH_CLOSE, structuringElement );
cv::Mat closed_thres;
cv::Mat structuringElement = getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(40, 40));
cv::morphologyEx( thres, closed_thres, cv::MORPH_CLOSE, structuringElement );
//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.
//eg. we draw to the output to be displayed in the main() function.
bool objectDetected = false;
Mat temp;
cv::Mat temp;
closed_thres.copyTo(temp);
//these two vectors needed for output of findContours
vector< vector<Point> > contours;
vector<Vec4i> hierarchy;
vector< vector<cv::Point> > contours;
vector<cv::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
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.size()>0)objectDetected=true;
@ -190,11 +189,11 @@ class Traite_image {
//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.
//this will be the object's final esticv::Mated position.
for(int i=0; i<contours.size();i++)
{
objectBoundingRectangle = boundingRect(contours[i]);
rectangle(output, objectBoundingRectangle, Scalar(0, 255, 0), 2);
objectBoundingRectangle = cv::boundingRect(contours[i]);
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
@ -204,36 +203,16 @@ class Traite_image {
//~ int height = objectBoundingRectangle.height;
//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
//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;
}
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);
}
};