feat(roi_cluster_fusion): fix geometry function and debugger in roi_cluster_fusion node

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/include/image_projection_based_fusion/utils/geometry.hpp

@@ -64,6 +64,8 @@ bool is_inside(
   const sensor_msgs::msg::RegionOfInterest & outer,
   const sensor_msgs::msg::RegionOfInterest & inner, const double outer_offset_scale = 1.1);
 
+void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width, const int height);
+
 }  // namespace image_projection_based_fusion
 
 #endif  // IMAGE_PROJECTION_BASED_FUSION__UTILS__GEOMETRY_HPP_

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());

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);

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) {
@@ -178,17 +177,18 @@ void RoiClusterFusionNode::fuseOnSingleImage(
       double iou(0.0);
       bool is_use_non_trust_object_iou_mode = is_far_enough(
         input_cluster_msg.feature_objects.at(cluster_map.first), trust_object_distance_);
+      auto image_roi = feature_obj.feature.roi;
+      auto cluster_roi = cluster_map.second;
+      sanitizeROI(image_roi, camera_info.width, camera_info.height);
+      sanitizeROI(cluster_roi, camera_info.width, camera_info.height);
       if (is_use_non_trust_object_iou_mode || is_roi_label_known) {
-        iou =
-          cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, non_trust_object_iou_mode_);
+        iou = cal_iou_by_mode(cluster_roi, image_roi, non_trust_object_iou_mode_);
       } else {
-        iou = cal_iou_by_mode(cluster_map.second, feature_obj.feature.roi, trust_object_iou_mode_);
+        iou = cal_iou_by_mode(cluster_roi, image_roi, trust_object_iou_mode_);
       }
 
       const bool passed_inside_cluster_gate =
-        only_allow_inside_cluster_
-          ? is_inside(feature_obj.feature.roi, cluster_map.second, roi_scale_factor_)
-          : true;
+        only_allow_inside_cluster_ ? is_inside(image_roi, cluster_roi, roi_scale_factor_) : true;
       if (max_iou < iou && passed_inside_cluster_gate) {
         index = cluster_map.first;
         max_iou = iou;
@@ -229,13 +229,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);
   }
 }

perception/image_projection_based_fusion/src/utils/geometry.cpp

@@ -166,11 +166,42 @@ bool is_inside(
   const sensor_msgs::msg::RegionOfInterest & outer,
   const sensor_msgs::msg::RegionOfInterest & inner, const double outer_offset_scale)
 {
-  const double lower_scale = 1.0 - std::abs(outer_offset_scale - 1.0);
-  return outer.x_offset * lower_scale <= inner.x_offset &&
-         outer.y_offset * lower_scale <= inner.y_offset &&
-         inner.x_offset + inner.width <= (outer.x_offset + outer.width) * outer_offset_scale &&
-         inner.y_offset + inner.height <= (outer.y_offset + outer.height) * outer_offset_scale;
+  const auto scaled_width = static_cast<double>(outer.width) * outer_offset_scale;
+  const auto scaled_height = static_cast<double>(outer.height) * outer_offset_scale;
+  const auto scaled_x_offset =
+    static_cast<double>(outer.x_offset) - (scaled_width - outer.width) / 2.0;
+  const auto scaled_y_offset =
+    static_cast<double>(outer.y_offset) - (scaled_height - outer.height) / 2.0;
+
+  // 1. check left-top corner
+  if (scaled_x_offset > inner.x_offset || scaled_y_offset > inner.y_offset) {
+    return false;
+  }
+  // 2. check right-bottom corner
+  if (
+    scaled_x_offset + scaled_width < inner.x_offset + inner.width ||
+    scaled_y_offset + scaled_height < inner.y_offset + inner.height) {
+    return false;
+  }
+  return true;
+}
+
+void sanitizeROI(sensor_msgs::msg::RegionOfInterest & roi, const int width_, const int height_)
+{
+  const unsigned int width = static_cast<unsigned int>(width_);
+  const unsigned int height = static_cast<unsigned int>(height_);
+  if (roi.x_offset >= width || roi.y_offset >= height) {
+    roi.width = 0;
+    roi.height = 0;
+    return;
+  }
+
+  if (roi.x_offset + roi.width > width) {
+    roi.width = width - roi.x_offset;
+  }
+  if (roi.y_offset + roi.height > height) {
+    roi.height = height - roi.y_offset;
+  }
 }
 
 }  // namespace image_projection_based_fusion