feat(image_projection_based_fusion): enable to show iou value in roi_…

…cluster_fusion debug image (autowarefoundation#5541) * enable to show debug iou value in roi_cluster_fusion Signed-off-by: yoshiri <[email protected]> * refactor iou draw settings Signed-off-by: yoshiri <[email protected]> * add backgroud color to iou Signed-off-by: yoshiri <[email protected]> * prevent object copying when debugger is not enabled Signed-off-by: yoshiri <[email protected]> --------- Signed-off-by: yoshiri <[email protected]>
satoshi-ota · Jan 14, 2024 · cfbf825 · cfbf825
1 parent d15fa6f
commit cfbf825
Show file tree

Hide file tree

Showing 4 changed files with 58 additions and 18 deletions.
diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/debugger.hpp
@@ -50,6 +50,7 @@ class Debugger
   std::vector<RegionOfInterest> image_rois_;
   std::vector<RegionOfInterest> obstacle_rois_;
   std::vector<Eigen::Vector2d> obstacle_points_;
+  std::vector<double> max_iou_for_image_rois_;
 
 private:
   void imageCallback(

diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp
@@ -77,36 +77,76 @@ void Debugger::clear()
   image_rois_.clear();
   obstacle_rois_.clear();
   obstacle_points_.clear();
+  max_iou_for_image_rois_.clear();
 }
 
 void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & stamp)
 {
   const boost::circular_buffer<sensor_msgs::msg::Image::ConstSharedPtr> & image_buffer =
     image_buffers_.at(image_id);
   const image_transport::Publisher & image_pub = image_pubs_.at(image_id);
+  const bool draw_iou_score =
+    max_iou_for_image_rois_.size() > 0 && max_iou_for_image_rois_.size() == image_rois_.size();
 
   for (std::size_t i = 0; i < image_buffer.size(); ++i) {
     if (image_buffer.at(i)->header.stamp != stamp) {
       continue;
     }
 
     auto cv_ptr = cv_bridge::toCvCopy(image_buffer.at(i), image_buffer.at(i)->encoding);
-
+    // draw obstacle points
     for (const auto & point : obstacle_points_) {
       cv::circle(
         cv_ptr->image, cv::Point(static_cast<int>(point.x()), static_cast<int>(point.y())), 2,
         cv::Scalar(255, 255, 255), 3, 4);
     }
+
+    // draw rois
+    const int img_height = static_cast<int>(image_buffer.at(i)->height);
+    const int img_width = static_cast<int>(image_buffer.at(i)->width);
     for (const auto & roi : obstacle_rois_) {
-      drawRoiOnImage(
-        cv_ptr->image, roi, image_buffer.at(i)->width, image_buffer.at(i)->height,
-        cv::Scalar(255, 0, 0));
+      drawRoiOnImage(cv_ptr->image, roi, img_width, img_height, cv::Scalar(255, 0, 0));  // blue
     }
-    // TODO(yukke42): show iou_score on image
     for (const auto & roi : image_rois_) {
-      drawRoiOnImage(
-        cv_ptr->image, roi, image_buffer.at(i)->width, image_buffer.at(i)->height,
-        cv::Scalar(0, 0, 255));
+      drawRoiOnImage(cv_ptr->image, roi, img_width, img_height, cv::Scalar(0, 0, 255));  // red
+    }
+
+    // show iou_score on image
+    if (draw_iou_score) {
+      for (auto roi_index = 0; roi_index < static_cast<int>(image_rois_.size()); ++roi_index) {
+        std::stringstream stream;
+        stream << std::fixed << std::setprecision(2) << max_iou_for_image_rois_.at(roi_index);
+        std::string iou_score = stream.str();
+
+        // set text position
+        int baseline = 3;
+        cv::Size textSize = cv::getTextSize(iou_score, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseline);
+        const int text_height = static_cast<int>(textSize.height);
+        const int text_width = static_cast<int>(textSize.width);
+        int x = image_rois_.at(roi_index).x_offset;
+        int y = image_rois_.at(roi_index).y_offset;  // offset for text
+        if (y < 0 + text_height)
+          y = text_height;  // if roi is on the top of image, put text on lower left of roi
+        if (y > img_height - text_height)
+          y = img_height -
+              text_height;  // if roi is on the bottom of image, put text on upper left of roi
+        if (x > img_width - text_width)
+          x = img_width - text_width;  // if roi is on the right of image, put text on left of roi
+        if (x < 0) x = 0;              // if roi is on the left of image, put text on right of roi
+
+        // choice color by iou score
+        // cv::Scalar color = max_iou_for_image_rois_.at(i) > 0.5 ? cv::Scalar(0, 255, 0) :
+        // cv::Scalar(0, 0, 255);
+        cv::Scalar color = cv::Scalar(0, 0, 255);  // red
+
+        cv::rectangle(
+          cv_ptr->image, cv::Point(x, y - textSize.height - baseline),
+          cv::Point(x + textSize.width, y), cv::Scalar(255, 255, 255),
+          cv::FILLED);  // white background
+        cv::putText(
+          cv_ptr->image, iou_score, cv::Point(x, y), cv::FONT_HERSHEY_SIMPLEX, 0.5, color, 1,
+          cv::LINE_AA);  // text
+      }
     }
 
     image_pub.publish(cv_ptr->toImageMsg());

diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp
@@ -203,6 +203,7 @@ void FusionNode<Msg, Obj>::subCallback(const typename Msg::ConstSharedPtr input_
     (*output_msg).header.stamp.sec * (int64_t)1e9 + (*output_msg).header.stamp.nanosec;
 
   // if matching rois exist, fuseOnSingle
+  // please ask maintainers before parallelize this loop because debugger is not thread safe
   for (std::size_t roi_i = 0; roi_i < rois_number_; ++roi_i) {
     if (camera_info_map_.find(roi_i) == camera_info_map_.end()) {
       RCLCPP_WARN(this->get_logger(), "no camera info. id is %zu", roi_i);

diff --git a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp
@@ -84,10 +84,6 @@ void RoiClusterFusionNode::fuseOnSingleImage(
   const DetectedObjectsWithFeature & input_roi_msg,
   const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg)
 {
-  std::vector<sensor_msgs::msg::RegionOfInterest> debug_image_rois;
-  std::vector<sensor_msgs::msg::RegionOfInterest> debug_pointcloud_rois;
-  std::vector<Eigen::Vector2d> debug_image_points;
-
   Eigen::Matrix4d projection;
   projection << camera_info.p.at(0), camera_info.p.at(1), camera_info.p.at(2), camera_info.p.at(3),
     camera_info.p.at(4), camera_info.p.at(5), camera_info.p.at(6), camera_info.p.at(7),
@@ -100,6 +96,9 @@ void RoiClusterFusionNode::fuseOnSingleImage(
       tf_buffer_, /*target*/ camera_info.header.frame_id,
       /*source*/ input_cluster_msg.header.frame_id, camera_info.header.stamp);
     if (!transform_stamped_optional) {
+      RCLCPP_WARN_STREAM(
+        get_logger(), "Failed to get transform from " << input_cluster_msg.header.frame_id << " to "
+                                                      << camera_info.header.frame_id);
       return;
     }
     transform_stamped = transform_stamped_optional.value();
@@ -151,7 +150,7 @@ void RoiClusterFusionNode::fuseOnSingleImage(
         max_x = std::max(static_cast<int>(normalized_projected_point.x()), max_x);
         max_y = std::max(static_cast<int>(normalized_projected_point.y()), max_y);
         projected_points.push_back(normalized_projected_point);
-        debug_image_points.push_back(normalized_projected_point);
+        if (debugger_) debugger_->obstacle_points_.push_back(normalized_projected_point);
       }
     }
     if (projected_points.empty()) {
@@ -165,7 +164,7 @@ void RoiClusterFusionNode::fuseOnSingleImage(
     roi.width = max_x - min_x;
     roi.height = max_y - min_y;
     m_cluster_roi.insert(std::make_pair(i, roi));
-    debug_pointcloud_rois.push_back(roi);
+    if (debugger_) debugger_->obstacle_rois_.push_back(roi);
   }
 
   for (const auto & feature_obj : input_roi_msg.feature_objects) {
@@ -229,13 +228,12 @@ void RoiClusterFusionNode::fuseOnSingleImage(
         }
       }
     }
-    debug_image_rois.push_back(feature_obj.feature.roi);
+    if (debugger_) debugger_->image_rois_.push_back(feature_obj.feature.roi);
+    if (debugger_) debugger_->max_iou_for_image_rois_.push_back(max_iou);
   }
 
+  // note: debug objects are safely cleared in fusion_node.cpp
   if (debugger_) {
-    debugger_->image_rois_ = debug_image_rois;
-    debugger_->obstacle_rois_ = debug_pointcloud_rois;
-    debugger_->obstacle_points_ = debug_image_points;
     debugger_->publishImage(image_id, input_roi_msg.header.stamp);
   }
 }