From cfbf8252d7a016e1a36898d5e0a025cc46fef6f1 Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Mon, 13 Nov 2023 20:32:22 +0900 Subject: [PATCH] feat(image_projection_based_fusion): enable to show iou value in roi_cluster_fusion debug image (#5541) * enable to show debug iou value in roi_cluster_fusion Signed-off-by: yoshiri * refactor iou draw settings Signed-off-by: yoshiri * add backgroud color to iou Signed-off-by: yoshiri * prevent object copying when debugger is not enabled Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- .../debugger.hpp | 1 + .../src/debugger.cpp | 56 ++++++++++++++++--- .../src/fusion_node.cpp | 1 + .../src/roi_cluster_fusion/node.cpp | 18 +++--- 4 files changed, 58 insertions(+), 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 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..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) @@ -84,6 +85,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) { @@ -91,22 +94,59 @@ 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); } + + // 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, 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(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(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; // 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 index 447e66f076c0b..0ed237f4752df 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(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 index 60070ad59b939..4bdd797df3a21 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,10 +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_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(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()) { @@ -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); } }