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