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

remove namespace cv

video-stab
samilyjcc 10 years ago
parent
ca53673111
commit
0e85119c64
1 changed files with 57 additions and 78 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. 133
      src/papillon.cpp

133
src/papillon.cpp
View File

@ -8,22 +8,21 @@
#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;
@ -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);
}
};

Write Preview
Loading…
Cancel
Save