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/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/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..5875f449cc56f 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) { @@ -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); } } 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