diff --git a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp index 2b79ef75..ec3a5e95 100644 --- a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp +++ b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp @@ -14,17 +14,22 @@ class Detection2DOverlay : public rclcpp::Node { explicit Detection2DOverlay(const rclcpp::NodeOptions& options); void onInit(); - void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); + void overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, + const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections); message_filters::Subscriber previewSub; + message_filters::Subscriber infoSub; message_filters::Subscriber detSub; - typedef message_filters::sync_policies::ApproximateTime syncPolicy; + typedef message_filters::sync_policies::ApproximateTime + syncPolicy; std::unique_ptr> sync; rclcpp::Publisher::SharedPtr overlayPub; std::vector labelMap = {"background", "aeroplane", "bicycle", "bird", "boat", "bottle", "bus", "car", "cat", "chair", "cow", "diningtable", "dog", "horse", "motorbike", "person", "pottedplant", "sheep", "sofa", "train", "tvmonitor"}; + bool desqueeze = false; }; -} // namespace depthai_filters \ No newline at end of file +} // namespace depthai_filters diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp index c6f667ab..ad1a3bf4 100644 --- a/depthai_filters/src/detection2d_overlay.cpp +++ b/depthai_filters/src/detection2d_overlay.cpp @@ -10,19 +10,43 @@ Detection2DOverlay::Detection2DOverlay(const rclcpp::NodeOptions& options) : rcl } void Detection2DOverlay::onInit() { previewSub.subscribe(this, "rgb/preview/image_raw"); + infoSub.subscribe(this, "stereo/camera_info"); detSub.subscribe(this, "nn/detections"); - sync = std::make_unique>(syncPolicy(10), previewSub, detSub); - sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2)); + sync = std::make_unique>(syncPolicy(10), previewSub, infoSub, detSub); + sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); overlayPub = this->create_publisher("overlay", 10); + desqueeze = this->declare_parameter("desqueeze", false); labelMap = this->declare_parameter>("label_map", labelMap); } void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr& preview, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info, const vision_msgs::msg::Detection2DArray::ConstSharedPtr& detections) { - cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); + cv::Mat previewMat = utils::msgToMat(this->get_logger(), preview, sensor_msgs::image_encodings::BGR8); auto blue = cv::Scalar(255, 0, 0); + double ratioX = 1.0; + double ratioY = 1.0; + int offsetX = 0; + double offsetY = 0; + // if preview size is less than camera info size + if(previewMat.rows < info->height || previewMat.cols < info->width) { + ratioY = double(info->height) / double(previewMat.rows); + if(desqueeze) { + ratioX = double(info->width) / double(previewMat.cols); + } else { + ratioX = ratioY; + offsetX = (info->width - info->height) / 2.0; + } + } else { + ratioY = double(previewMat.rows) / double(info->height); + if(desqueeze) { + ratioX = double(previewMat.cols) / double(info->width); + } else { + ratioX = double(previewMat.cols) / double(info->width); + } + } for(auto& detection : detections->detections) { auto x1 = detection.bbox.center.position.x - detections->detections[0].bbox.size_x / 2.0; auto x2 = detection.bbox.center.position.x + detections->detections[0].bbox.size_x / 2.0; @@ -30,6 +54,10 @@ void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr auto y2 = detection.bbox.center.position.y + detections->detections[0].bbox.size_y / 2.0; auto labelStr = labelMap[stoi(detection.results[0].hypothesis.class_id)]; auto confidence = detection.results[0].hypothesis.score; + x1 = x1 * ratioX + offsetX; + x2 = x2 * ratioX + offsetX; + y1 = y1 * ratioY + offsetY; + y2 = y2 * ratioY + offsetY; utils::addTextToFrame(previewMat, labelStr, x1 + 10, y1 + 20); std::stringstream confStr; confStr << std::fixed << std::setprecision(2) << confidence * 100; @@ -44,4 +72,4 @@ void Detection2DOverlay::overlayCB(const sensor_msgs::msg::Image::ConstSharedPtr } // namespace depthai_filters #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Detection2DOverlay); \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(depthai_filters::Detection2DOverlay); diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp index 3986f6c4..0248fce8 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/detection.hpp @@ -84,9 +84,11 @@ class Detection : public BaseNode { convConf.updateROSBaseTimeOnRosMsg = ph->getParam("i_update_ros_base_time_on_ros_msg"); utils::ImgPublisherConfig pubConf; + pubConf.width = width; + pubConf.height = height; pubConf.daiNodeName = getName(); pubConf.topicName = "~/" + getName(); - pubConf.topicSuffix = "passthrough"; + pubConf.topicSuffix = "/passthrough"; pubConf.socket = static_cast(ph->getParam("i_board_socket_id")); ptPub->setup(device, convConf, pubConf); diff --git a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp index f22d1986..a72bea2d 100644 --- a/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp +++ b/depthai_ros_driver/include/depthai_ros_driver/dai_nodes/nn/spatial_detection.hpp @@ -77,7 +77,7 @@ class SpatialDetection : public BaseNode { pubConf.height = height; pubConf.daiNodeName = getName(); pubConf.topicName = "~/" + getName(); - pubConf.topicSuffix = "passthrough"; + pubConf.topicSuffix = "/passthrough"; pubConf.socket = static_cast(ph->getParam("i_board_socket_id")); ptPub->setup(device, convConf, pubConf);