From 73fe8a932537d9c16df75a4c6e8f9181a7cc3b11 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 10 Nov 2023 00:32:54 +0900 Subject: [PATCH 1/5] enable to show debug iou value in roi_cluster_fusion Signed-off-by: yoshiri --- .../debugger.hpp | 1 + .../src/debugger.cpp | 37 +++++++++++++++++-- .../src/roi_cluster_fusion/node.cpp | 6 +++ 3 files changed, 41 insertions(+), 3 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 index 8a6ac7672b3a8..e4b1913effed5 100644 --- 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 image_rois_; std::vector obstacle_rois_; std::vector obstacle_points_; + std::vector 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 index 3f52a0de1f09d..69d90d7d548a8 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -84,6 +84,8 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta const boost::circular_buffer & 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) { @@ -103,10 +105,39 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta cv::Scalar(255, 0, 0)); } // TODO(yukke42): show iou_score on image + const int img_height = static_cast(image_buffer.at(i)->height); + const int img_width = static_cast(image_buffer.at(i)->width); + if (draw_iou_score) { + for (auto roi_index = 0; roi_index < static_cast(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 x = image_rois_.at(roi_index).x_offset; + int y = image_rois_.at(roi_index).y_offset - 5; // offset for text + if (y < 0) + y = image_rois_.at(roi_index).y_offset + + 20; // if roi is on the top of image, put text on lower left of roi + if (y > img_height) + y = img_height - 5; // if roi is on the bottom of image, put text on upper left of roi + if (x > img_width) + x = img_width - 50; // if roi is on the right of image, put text on left of roi + if (x < 0) x = img_width - 50; // 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::putText( + cv_ptr->image, iou_score, cv::Point(x, y), cv::FONT_HERSHEY_SIMPLEX, 0.5, color, 1, + cv::LINE_AA); + } + } + 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)); } image_pub.publish(cv_ptr->toImageMsg()); 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 index 4a46c8aace696..88aa30e5e9f6e 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -86,6 +86,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( { std::vector debug_image_rois; std::vector debug_pointcloud_rois; + std::vector debug_max_iou_for_image_rois; std::vector debug_image_points; Eigen::Matrix4d projection; @@ -100,6 +101,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(); @@ -232,12 +236,14 @@ void RoiClusterFusionNode::fuseOnSingleImage( } } 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); } } From f0da0a5f387e679677ab7e2be07762fc09c47ef1 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 10 Nov 2023 02:38:42 +0900 Subject: [PATCH 2/5] refactor iou draw settings Signed-off-by: yoshiri --- .../src/debugger.cpp | 46 ++++++++++--------- 1 file changed, 25 insertions(+), 21 deletions(-) diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index 69d90d7d548a8..ceb37fe5d995d 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -93,20 +93,24 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta } 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(point.x()), static_cast(point.y())), 2, cv::Scalar(255, 255, 255), 3, 4); } - 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)); - } - // TODO(yukke42): show iou_score on image + + // draw rois const int img_height = static_cast(image_buffer.at(i)->height); const int img_width = static_cast(image_buffer.at(i)->width); + for (const auto & roi : obstacle_rois_) { + drawRoiOnImage(cv_ptr->image, roi, img_width, img_height, cv::Scalar(255, 0, 0)); // blue + } + for (const auto & roi : image_rois_) { + 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(image_rois_.size()); ++roi_index) { std::stringstream stream; @@ -114,16 +118,20 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta 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(textSize.height); + const int text_width = static_cast(textSize.width); int x = image_rois_.at(roi_index).x_offset; - int y = image_rois_.at(roi_index).y_offset - 5; // offset for text - if (y < 0) - y = image_rois_.at(roi_index).y_offset + - 20; // if roi is on the top of image, put text on lower left of roi - if (y > img_height) - y = img_height - 5; // if roi is on the bottom of image, put text on upper left of roi - if (x > img_width) - x = img_width - 50; // if roi is on the right of image, put text on left of roi - if (x < 0) x = img_width - 50; // if roi is on the left of image, put text on right of roi + 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) : @@ -132,14 +140,10 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta cv::putText( cv_ptr->image, iou_score, cv::Point(x, y), cv::FONT_HERSHEY_SIMPLEX, 0.5, color, 1, - cv::LINE_AA); + cv::LINE_AA); // text } } - for (const auto & roi : image_rois_) { - drawRoiOnImage(cv_ptr->image, roi, img_width, img_height, cv::Scalar(0, 0, 255)); - } - image_pub.publish(cv_ptr->toImageMsg()); break; } From fc57a64201ad87b37573a67e4f3f625a3152e9e7 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 10 Nov 2023 02:39:05 +0900 Subject: [PATCH 3/5] add backgroud color to iou Signed-off-by: yoshiri --- perception/image_projection_based_fusion/src/debugger.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index ceb37fe5d995d..0918d2ed291a8 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -138,6 +138,10 @@ void Debugger::publishImage(const std::size_t image_id, const rclcpp::Time & sta // 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 From 887eca8c88d44c239f51ab4a9576b96b91a175d2 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Mon, 13 Nov 2023 18:59:52 +0900 Subject: [PATCH 4/5] prevent object copying when debugger is not enabled Signed-off-by: yoshiri --- .../src/debugger.cpp | 1 + .../src/fusion_node.cpp | 1 + .../src/roi_cluster_fusion/node.cpp | 18 +++++------------- 3 files changed, 7 insertions(+), 13 deletions(-) diff --git a/perception/image_projection_based_fusion/src/debugger.cpp b/perception/image_projection_based_fusion/src/debugger.cpp index 0918d2ed291a8..a51c23a77aac6 100644 --- a/perception/image_projection_based_fusion/src/debugger.cpp +++ b/perception/image_projection_based_fusion/src/debugger.cpp @@ -77,6 +77,7 @@ 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) diff --git a/perception/image_projection_based_fusion/src/fusion_node.cpp b/perception/image_projection_based_fusion/src/fusion_node.cpp index a540688f7e751..df797369208fe 100644 --- 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::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_THROTTLE( 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 index 88aa30e5e9f6e..9da9df7eff6ca 100644 --- 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,11 +84,6 @@ void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_roi_msg, const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) { - std::vector debug_image_rois; - std::vector debug_pointcloud_rois; - std::vector debug_max_iou_for_image_rois; - std::vector 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), @@ -155,7 +150,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( max_x = std::max(static_cast(normalized_projected_point.x()), max_x); max_y = std::max(static_cast(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()) { @@ -169,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) { @@ -235,15 +230,12 @@ void RoiClusterFusionNode::fuseOnSingleImage( } } } - debug_image_rois.push_back(feature_obj.feature.roi); - debug_max_iou_for_image_rois.push_back(max_iou); + 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_->max_iou_for_image_rois_ = debug_max_iou_for_image_rois; debugger_->publishImage(image_id, input_roi_msg.header.stamp); } } From 66c556fb22d4418846458aa6301079c8a55c5e19 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Mon, 13 Nov 2023 20:42:42 +0900 Subject: [PATCH 5/5] fix roi geometry functions Signed-off-by: yoshiri --- .../utils/geometry.hpp | 2 + .../src/roi_cluster_fusion/node.cpp | 13 +++--- .../src/utils/geometry.cpp | 41 ++++++++++++++++--- 3 files changed, 45 insertions(+), 11 deletions(-) diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/geometry.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/geometry.hpp index 199246715551c..de6a8c2987821 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/utils/geometry.hpp +++ b/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_ 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 index 9da9df7eff6ca..492f832e30e8b 100644 --- a/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp +++ b/perception/image_projection_based_fusion/src/roi_cluster_fusion/node.cpp @@ -177,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; diff --git a/perception/image_projection_based_fusion/src/utils/geometry.cpp b/perception/image_projection_based_fusion/src/utils/geometry.cpp index 9ea49256650ee..29198280ec7b2 100644 --- a/perception/image_projection_based_fusion/src/utils/geometry.cpp +++ b/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(outer.width) * outer_offset_scale; + const auto scaled_height = static_cast(outer.height) * outer_offset_scale; + const auto scaled_x_offset = + static_cast(outer.x_offset) - (scaled_width - outer.width) / 2.0; + const auto scaled_y_offset = + static_cast(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(width_); + const unsigned int height = static_cast(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