detection2d overlay update

luxonis · Sep 27, 2024 · dac67c9 · dac67c9
1 parent d342ac7
commit dac67c9
Show file tree

Hide file tree

Showing 4 changed files with 44 additions and 9 deletions.
diff --git 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<sensor_msgs::msg::Image> previewSub;
+    message_filters::Subscriber<sensor_msgs::msg::CameraInfo> infoSub;
     message_filters::Subscriber<vision_msgs::msg::Detection2DArray> detSub;
 
-    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, vision_msgs::msg::Detection2DArray> syncPolicy;
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, vision_msgs::msg::Detection2DArray>
+        syncPolicy;
     std::unique_ptr<message_filters::Synchronizer<syncPolicy>> sync;
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr overlayPub;
     std::vector<std::string> 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
+}  // namespace depthai_filters
diff --git a/depthai_filters/src/detection2d_overlay.cpp b/depthai_filters/src/detection2d_overlay.cpp
@@ -10,26 +10,54 @@ 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<message_filters::Synchronizer<syncPolicy>>(syncPolicy(10), previewSub, detSub);
-    sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2));
+    sync = std::make_unique<message_filters::Synchronizer<syncPolicy>>(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<sensor_msgs::msg::Image>("overlay", 10);
+    desqueeze = this->declare_parameter<bool>("desqueeze", false);
     labelMap = this->declare_parameter<std::vector<std::string>>("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;
         auto y1 = detection.bbox.center.position.y - detections->detections[0].bbox.size_y / 2.0;
         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);
+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
@@ -84,9 +84,11 @@ class Detection : public BaseNode {
             convConf.updateROSBaseTimeOnRosMsg = ph->getParam<bool>("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<dai::CameraBoardSocket>(ph->getParam<int>("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
@@ -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<dai::CameraBoardSocket>(ph->getParam<int>("i_board_socket_id"));
 
             ptPub->setup(device, convConf, pubConf);