detection overlay update

luxonis · Sep 27, 2024 · f803ecd · f803ecd
1 parent 93b29b9
commit f803ecd
Show file tree

Hide file tree

Showing 5 changed files with 44 additions and 8 deletions.
diff --git a/depthai_filters/CMakeLists.txt b/depthai_filters/CMakeLists.txt
@@ -4,6 +4,7 @@ project(depthai_filters VERSION 2.10.2 LANGUAGES CXX C)
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 add_compile_options(-g)
 add_definitions(-std=c++14)
 

diff --git a/depthai_filters/include/depthai_filters/detection2d_overlay.hpp b/depthai_filters/include/depthai_filters/detection2d_overlay.hpp
@@ -7,23 +7,26 @@
 #include "nodelet/nodelet.h"
 #include "ros/ros.h"
 #include "sensor_msgs/Image.h"
+#include "sensor_msgs/CameraInfo.h"
 #include "vision_msgs/Detection2DArray.h"
 
 namespace depthai_filters {
 class Detection2DOverlay : public nodelet::Nodelet {
    public:
     void onInit() override;
 
-    void overlayCB(const sensor_msgs::ImageConstPtr& preview, const vision_msgs::Detection2DArrayConstPtr& detections);
+    void overlayCB(const sensor_msgs::ImageConstPtr& preview, const sensor_msgs::CameraInfoConstPtr& info, const vision_msgs::Detection2DArrayConstPtr& detections);
 
     message_filters::Subscriber<sensor_msgs::Image> previewSub;
+	message_filters::Subscriber<sensor_msgs::CameraInfo> infoSub;
     message_filters::Subscriber<vision_msgs::Detection2DArray> detSub;
 
-    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, vision_msgs::Detection2DArray> syncPolicy;
+    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::CameraInfo, vision_msgs::Detection2DArray> syncPolicy;
     std::unique_ptr<message_filters::Synchronizer<syncPolicy>> sync;
     ros::Publisher 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
@@ -7,31 +7,61 @@
 #include "depthai_filters/utils.hpp"
 #include "nodelet/nodelet.h"
 #include "pluginlib/class_list_macros.h"
+#include "sensor_msgs/CameraInfo.h"
 
 namespace depthai_filters {
 void Detection2DOverlay::onInit() {
     auto pNH = getPrivateNodeHandle();
     previewSub.subscribe(pNH, "/rgb/preview/image_raw", 1);
     detSub.subscribe(pNH, "/nn/detections", 1);
-    sync = std::make_unique<message_filters::Synchronizer<syncPolicy>>(syncPolicy(10), previewSub, detSub);
+    infoSub.subscribe(pNH, "rgb/preview/camera_info", 1);
+    sync = std::make_unique<message_filters::Synchronizer<syncPolicy>>(syncPolicy(10), previewSub, infoSub, detSub);
     pNH.getParam("label_map", labelMap);
-    sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2));
+    pNH.getParam("desqueeze", desqueeze);
+    sync->registerCallback(std::bind(&Detection2DOverlay::overlayCB, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
     overlayPub = pNH.advertise<sensor_msgs::Image>("overlay", 10);
 }
 
-void Detection2DOverlay::overlayCB(const sensor_msgs::ImageConstPtr& preview, const vision_msgs::Detection2DArrayConstPtr& detections) {
+void Detection2DOverlay::overlayCB(const sensor_msgs::ImageConstPtr& preview,
+                                   const sensor_msgs::CameraInfoConstPtr& info,
+                                   const vision_msgs::Detection2DArrayConstPtr& detections) {
     cv::Mat previewMat = utils::msgToMat(preview, sensor_msgs::image_encodings::BGR8);
     auto white = cv::Scalar(255, 255, 255);
     auto black = cv::Scalar(0, 0, 0);
     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.x - detections->detections[0].bbox.size_x / 2.0;
         auto x2 = detection.bbox.center.x + detections->detections[0].bbox.size_x / 2.0;
         auto y1 = detection.bbox.center.y - detections->detections[0].bbox.size_y / 2.0;
         auto y2 = detection.bbox.center.y + detections->detections[0].bbox.size_y / 2.0;
         auto labelStr = labelMap[detection.results[0].id];
         auto confidence = detection.results[0].score;
+        x1 = x1 * ratioX + offsetX;
+        x2 = x2 * ratioX + offsetX;
+        y1 = y1 * ratioY + offsetY;
+        y2 = y2 * ratioY + offsetY;
         cv::putText(previewMat, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, white, 3);
         cv::putText(previewMat, labelStr, cv::Point(x1 + 10, y1 + 20), cv::FONT_HERSHEY_TRIPLEX, 0.5, black);
         std::stringstream confStr;

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
@@ -86,9 +86,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
@@ -80,7 +80,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);