void CudaRosTest::imageCB(
const sensor_msgs::Image::ConstPtr &image_msg) {
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(
image_msg, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat image = cv_ptr->image.clone();
if (image.empty()) {
ROS_ERROR("EMPTY INPUT IMAGE");
return;
}
// cv::cvtColor(image, image, CV_BGR2GRAY);
// boxFilter(image, 23);
// boxFilter2D(image, 3);
boxFilterMan(image, 12);
// cv::cvtColor(image, image, CV_GRAY2BGR);
cv::namedWindow("image", cv::WINDOW_NORMAL);
cv::imshow("image", image);
cv::waitKey(3);
cv_bridge::CvImagePtr pub_msg(new cv_bridge::CvImage);
pub_msg->header = image_msg->header;
pub_msg->encoding = sensor_msgs::image_encodings::BGR8;
pub_msg->image = image.clone();
this->pub_image_.publish(pub_msg);
}
void UAVTracker::callback(const sensor_msgs::Image::ConstPtr &image_msg) {
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(
image_msg, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
if (!object_init_) {
ROS_WARN_ONCE("PLEASE INITIALIZE THE OBJECT");
return;
}
cv::Mat image = cv_ptr->image;
cv::Point2f init_tl = cv::Point2f(this->screen_rect_.x,
this->screen_rect_.y);
cv::Point2f init_br = cv::Point2f(init_tl.x + this->screen_rect_.width,
init_tl.y + this->screen_rect_.height);
cv::Mat im_gray;
cv::Mat img = image.clone();
cv::cvtColor(image, im_gray, CV_RGB2GRAY);
if (!tracker_init_) {
this->initialise(im_gray, init_tl, init_br);
this->tracker_init_ = true;
}
this->processFrame(im_gray);
for (int i = 0; i < this->trackedKeypoints.size(); i++) {
cv::circle(img, this->trackedKeypoints[i].first.pt,
3, cv::Scalar(255, 255, 255));
}
cv::Scalar color = cv::Scalar(0, 255, 0);
cv::line(img, this->topLeft, this->topRight, color, 3);
cv::line(img, this->topRight, this->bottomRight, color, 3);
cv::line(img, this->bottomRight, this->bottomLeft, color, 3);
cv::line(img, this->bottomLeft, this->topLeft, color, 3);
jsk_recognition_msgs::Rect jsk_rect;
jsk_rect.x = this->topLeft.x;
jsk_rect.y = this->topLeft.y;
jsk_rect.width = (this->bottomRight.x - jsk_rect.x);
jsk_rect.height = (this->bottomRight.y - jsk_rect.y);
this->pub_rect_.publish(jsk_rect);
cv_bridge::CvImagePtr pub_msg(new cv_bridge::CvImage);
pub_msg->header = image_msg->header;
pub_msg->encoding = sensor_msgs::image_encodings::BGR8;
pub_msg->image = img.clone();
this->pub_image_.publish(pub_msg);
// cv::imshow("image", img);
// cv::waitKey(3);
}
void ParticleFilterGPU::imageCB(
const sensor_msgs::Image::ConstPtr &image_msg) {
cv_bridge::CvImagePtr cv_ptr;
try {
cv_ptr = cv_bridge::toCvCopy(
image_msg, sensor_msgs::image_encodings::BGR8);
} catch (cv_bridge::Exception& e) {
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Mat image = cv_ptr->image.clone();
if (image.empty()) {
ROS_ERROR("EMPTY INPUT IMAGE");
return;
}
if (downsize_ > 1) {
cv::resize(image, image, cv::Size(image.cols/this->downsize_,
image.rows/this->downsize_));
}
if (tracker_init_) {
particleFilterGPU(image, screen_rect_, gpu_init_);
} else {
ROS_ERROR("THE TRACKER IS NOT INITALIZED");
}
cv_bridge::CvImagePtr pub_msg(new cv_bridge::CvImage);
pub_msg->header = image_msg->header;
pub_msg->encoding = sensor_msgs::image_encodings::BGR8;
pub_msg->image = image.clone();
this->pub_image_.publish(pub_msg);
cv::namedWindow("Tracking", cv::WINDOW_NORMAL);
cv::imshow("image", image);
cv::waitKey(3);
image.release();
}
