From f71802edb31dcf3ad55d3b8abff8d428b837d20d Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 22 Nov 2022 22:37:52 +0900 Subject: [PATCH 1/2] enable to set tracking rectangle from dynamic reconfigure --- cfg/CamShift.cfg | 5 +++++ src/nodelet/camshift_nodelet.cpp | 22 +++++++++++++++++++++- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/cfg/CamShift.cfg b/cfg/CamShift.cfg index b7b2b9ad..f97d69f2 100755 --- a/cfg/CamShift.cfg +++ b/cfg/CamShift.cfg @@ -43,4 +43,9 @@ gen.add("vmin", int_t, 0, "Vmin", 10, 1, 256) gen.add("vmax", int_t, 0, "Vmax", 256, 1, 256) gen.add("smin", int_t, 0, "Smin", 30, 1, 256) +gen.add("x", int_t, 0, "Selected object x (left)", 310, 1, 2048) +gen.add("y", int_t, 0, "Selected object y (top)", 230, 1, 1536) +gen.add("width", int_t, 0, "Selected object width", 20, 1, 2048) +gen.add("height", int_t, 0, "Selected object height", 20, 1, 1536) + exit(gen.generate(PACKAGE, "camshift", "CamShift")) diff --git a/src/nodelet/camshift_nodelet.cpp b/src/nodelet/camshift_nodelet.cpp index 5d139642..442b0014 100644 --- a/src/nodelet/camshift_nodelet.cpp +++ b/src/nodelet/camshift_nodelet.cpp @@ -72,6 +72,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet Config config_; boost::shared_ptr reconfigure_server_; + boost::mutex mutex_; int queue_size_; bool debug_view_; ros::Time prev_stamp_; @@ -86,6 +87,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet int vmin_, vmax_, smin_; bool backprojMode; bool selectObject; + int showSelected; int trackObject; bool showHist; cv::Point origin; @@ -109,10 +111,18 @@ class CamShiftNodelet : public opencv_apps::Nodelet void reconfigureCallback(Config& new_config, uint32_t level) { + boost::mutex::scoped_lock lock(mutex_); config_ = new_config; vmin_ = config_.vmin; vmax_ = config_.vmax; smin_ = config_.smin; + + selection.x = config_.x; + selection.y = config_.y; + selection.width = config_.width; + selection.height = config_.height; + selectObject = false; + trackObject = -1; } const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame) @@ -139,6 +149,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg) { + boost::mutex::scoped_lock lock(mutex_); // Work on the image. try { @@ -187,6 +198,12 @@ class CamShiftNodelet : public opencv_apps::Nodelet selection.height = std::abs(y - origin.y); selection &= cv::Rect(0, 0, frame.cols, frame.rows); + + config_.x = selection.x; + config_.y = selection.y; + config_.width = selection.width; + config_.height = selection.height; + reconfigure_server_->updateConfig(config_); } switch (event) @@ -234,6 +251,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet trackWindow = selection; trackObject = 1; + showSelected = 10; histimg = cv::Scalar::all(0); int bin_w = histimg.cols / hsize; @@ -284,11 +302,12 @@ class CamShiftNodelet : public opencv_apps::Nodelet else if (trackObject < 0) paused = false; - if (selectObject && selection.width > 0 && selection.height > 0) + if ((showSelected > 0 || selectObject) && selection.width > 0 && selection.height > 0) { cv::Mat roi(frame, selection); bitwise_not(roi, roi); } + if ( showSelected > 0 ) showSelected--; if (debug_view_) { @@ -376,6 +395,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet smin_ = 30; backprojMode = false; selectObject = false; + showSelected = 0; trackObject = 0; showHist = true; paused = false; From 320229fc2c9a5c29b6f182d1f1bf581ed68cf762 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Tue, 22 Nov 2022 23:13:03 +0900 Subject: [PATCH 2/2] opencv_apps/camshift: publish confidence of tracking --- src/nodelet/camshift_nodelet.cpp | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/src/nodelet/camshift_nodelet.cpp b/src/nodelet/camshift_nodelet.cpp index 442b0014..0e4f186d 100644 --- a/src/nodelet/camshift_nodelet.cpp +++ b/src/nodelet/camshift_nodelet.cpp @@ -53,6 +53,7 @@ #include #include +#include #include "opencv_apps/CamShiftConfig.h" #include "opencv_apps/RotatedRectStamped.h" @@ -63,7 +64,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet image_transport::Publisher img_pub_, bproj_pub_; image_transport::Subscriber img_sub_; image_transport::CameraSubscriber cam_sub_; - ros::Publisher msg_pub_; + ros::Publisher msg_pub_, conf_pub_; boost::shared_ptr it_; @@ -348,6 +349,28 @@ class CamShiftNodelet : public opencv_apps::Nodelet bproj_pub_.publish(out_img2); if (trackObject) msg_pub_.publish(rect_msg); + // publish cnofidence + { + cv::RotatedRect track_box; + track_box.angle = rect_msg.rect.angle; + track_box.center.x = rect_msg.rect.center.x; + track_box.center.y = rect_msg.rect.center.y; + track_box.size.width = rect_msg.rect.size.width; + track_box.size.height = rect_msg.rect.size.height; + cv::Mat mask = backproj.clone(); + mask.setTo(cv::Scalar(0)); +#ifndef CV_VERSION_EPOCH + cv::ellipse(mask, track_box, 255, -1, cv::LINE_AA); +#else + cv::ellipse(mask, track_box, 255, -1, CV_AA); +#endif + cv::bitwise_and(backproj, mask, backproj); + double confidence = cv::countNonZero(mask); + if ( confidence > 0 ) { confidence = cv::countNonZero(backproj) / confidence; } + std_msgs::Float64 msg; msg.data = confidence; + conf_pub_.publish(msg); + NODELET_DEBUG("Confidence of tracking %f", confidence); + } } catch (cv::Exception& e) { @@ -413,6 +436,7 @@ class CamShiftNodelet : public opencv_apps::Nodelet img_pub_ = advertiseImage(*pnh_, "image", 1); bproj_pub_ = advertiseImage(*pnh_, "back_project", 1); + conf_pub_ = advertise(*pnh_, "confidence", 1); msg_pub_ = advertise(*pnh_, "track_box", 1); NODELET_INFO("Hot keys: ");