feat(image_projection_based_fusion): enable to show iou value in roi_cluster_fusion debug image

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(

perception/image_projection_based_fusion/src/debugger.cpp

@@ -84,29 +84,68 @@
   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());

perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp

@@ -86,20 +86,24 @@
 {
   std::vector<sensor_msgs::msg::RegionOfInterest> debug_image_rois;
   std::vector<sensor_msgs::msg::RegionOfInterest> debug_pointcloud_rois;
+  std::vector<double> debug_max_iou_for_image_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),
     camera_info.p.at(8), camera_info.p.at(9), camera_info.p.at(10), camera_info.p.at(11);
 
   // get transform from cluster frame id to camera optical frame id
   geometry_msgs::msg::TransformStamped transform_stamped;
   {
     const auto transform_stamped_optional = getTransformStamped(
       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();
@@ -232,12 +236,14 @@
       }
     }
     debug_image_rois.push_back(feature_obj.feature.roi);
+    debug_max_iou_for_image_rois.push_back(max_iou);
   }
 
   if (debugger_) {
     debugger_->image_rois_ = debug_image_rois;
     debugger_->obstacle_rois_ = debug_pointcloud_rois;
     debugger_->obstacle_points_ = debug_image_points;
+    debugger_->max_iou_for_image_rois_ = debug_max_iou_for_image_rois;
     debugger_->publishImage(image_id, input_roi_msg.header.stamp);
   }
 }