FIx crop ratio
This commit is contained in:
parent
d1687fb7cb
commit
37816cd161
1 changed files with 18 additions and 13 deletions
|
@ -13,10 +13,10 @@ using namespace std;
|
|||
|
||||
class Traite_image {
|
||||
public:
|
||||
const static int SENSITIVITY_VALUE = 30;
|
||||
const static int SENSITIVITY_VALUE = 40;
|
||||
const static int BLUR_SIZE = 10;
|
||||
|
||||
|
||||
|
||||
|
||||
Mat prev;
|
||||
Mat last_T;
|
||||
bool first = true;
|
||||
|
@ -67,16 +67,21 @@ class Traite_image {
|
|||
|
||||
Mat next_stab;
|
||||
stabiliseImg(prev, next, next_stab);
|
||||
Rect myROI(next_stab.size().width/8, next_stab.size().height/8, next_stab.size().width*3/4, next_stab.size().height*3/4);
|
||||
Mat next_stab_cropped = next_stab(myROI);
|
||||
Mat prev_cropped = prev(myROI);
|
||||
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);
|
||||
searchForMovement(prev_cropped, next_stab_cropped, output);
|
||||
|
||||
|
||||
pub_img.publish(cv_bridge::CvImage(msg->header, "rgb8", output).toImageMsg());
|
||||
// bridge_input is handled by a smart-pointer. No explicit delete needed.
|
||||
|
||||
droneTracking(Rect(Point(0,0), output.size()));
|
||||
|
||||
//droneTracking(Rect(Point(0,0), output.size()));
|
||||
|
||||
//ROS_INFO("pub");
|
||||
|
||||
|
@ -186,14 +191,14 @@ class Traite_image {
|
|||
return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;
|
||||
}
|
||||
|
||||
|
||||
void droneTracking(Rect img_size)
|
||||
|
||||
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;
|
||||
|
@ -202,7 +207,7 @@ class Traite_image {
|
|||
{
|
||||
twist.angular.z = -0.2;
|
||||
}
|
||||
|
||||
|
||||
pub_cmd.publish(twist);
|
||||
}
|
||||
};
|
||||
|
|
Loading…
Reference in a new issue