A bit of cleaning, simul arg
This commit is contained in:
parent
d8a97dbbdd
commit
c608313530
1 changed files with 23 additions and 15 deletions
|
@ -17,12 +17,13 @@ class Traite_image {
|
|||
const static int THRESHOLD_DETECT_SENSITIVITY = 10;
|
||||
const static int BLUR_SIZE = 5;
|
||||
const static int THRESHOLD_MOV = 5;
|
||||
const static int crop_ratio = 8;
|
||||
|
||||
|
||||
Mat prev;
|
||||
Mat last_T;
|
||||
bool first = true;
|
||||
int resize_f = 2;
|
||||
int resize_f = 1;
|
||||
|
||||
int theObject[2] = {0,0};
|
||||
Rect objectBoundingRectangle = Rect(0,0,0,0);
|
||||
|
@ -35,10 +36,22 @@ class Traite_image {
|
|||
image_transport::Subscriber sub;
|
||||
|
||||
|
||||
Traite_image() : n("~"),it(n) {
|
||||
pub_img = it.advertise("/image_out", 1);
|
||||
pub_cmd = n.advertise<geometry_msgs::Twist>("/vrep/drone/cmd_vel", 1);
|
||||
sub = it.subscribe("/usb_cam/image_raw", 1, [this](const sensor_msgs::ImageConstPtr& img) -> void { this->on_image(img);},ros::VoidPtr(),image_transport::TransportHints("compressed"));
|
||||
Traite_image(bool sim) : n("~"),it(n) {
|
||||
String img_out, cmd_out, img_in;
|
||||
if (!sim) {
|
||||
img_out = "/image_out";
|
||||
cmd_out = "/bebop/cmd_vel";
|
||||
img_in = "/bebop/image_raw";
|
||||
}
|
||||
else
|
||||
{
|
||||
img_out = "/image_out";
|
||||
cmd_out = "/vrep/drone/cmd_vel";
|
||||
img_in = "/usb_cam/image_raw";
|
||||
}
|
||||
pub_img = it.advertise(img_out, 1);
|
||||
pub_cmd = n.advertise<geometry_msgs::Twist>(cmd_out, 1);
|
||||
sub = it.subscribe(img_in, 1, [this](const sensor_msgs::ImageConstPtr& img) -> void { this->on_image(img);},ros::VoidPtr(),image_transport::TransportHints("compressed"));
|
||||
}
|
||||
|
||||
|
||||
|
@ -69,7 +82,6 @@ class Traite_image {
|
|||
|
||||
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);
|
||||
|
@ -77,7 +89,7 @@ class Traite_image {
|
|||
Rect myROI(crop_x, crop_y, crop_w, crop_h);
|
||||
Mat next_stab_cropped = next_stab(myROI);
|
||||
Mat prev_cropped = prev(myROI);
|
||||
searchForMovement(prev_cropped, next_stab_cropped, output);
|
||||
searchForMovementOptFlow(prev_cropped, next_stab_cropped, output);
|
||||
|
||||
|
||||
pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
|
||||
|
@ -207,7 +219,6 @@ class Traite_image {
|
|||
threshold(flow_norm, flow_norm, THRESHOLD_DETECT_SENSITIVITY, 255, THRESH_BINARY);
|
||||
flow_norm.convertTo(flow_norm, CV_8U);
|
||||
|
||||
bool objectDetected = false;
|
||||
Mat temp;
|
||||
flow_norm.copyTo(temp);
|
||||
//these two vectors needed for output of findContours
|
||||
|
@ -217,11 +228,7 @@ class Traite_image {
|
|||
//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){
|
||||
if(contours.size()>0){ //if contours vector is not empty, we have found some objects
|
||||
//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;
|
||||
|
@ -273,8 +280,9 @@ class Traite_image {
|
|||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "test_opencv");
|
||||
Traite_image dataset=Traite_image();
|
||||
ros::init(argc, argv, "Papillon");
|
||||
bool sim = false;
|
||||
Traite_image dataset=Traite_image(sim);
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
|
|
Loading…
Reference in a new issue