diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 51359d8d8f0e2..67e123ea5b018 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -15,7 +15,8 @@ - + + @@ -169,7 +170,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 45cfb91bf25c1..fbe5f21c041b6 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -15,7 +15,8 @@ - + + @@ -190,7 +191,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 77a3e345ec5e8..1fcf29606891b 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -35,7 +35,8 @@ - + + @@ -64,7 +65,8 @@ - + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index 3d4ce1035e72c..bbb7ebb2d8d25 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -93,7 +93,8 @@ - + + @@ -169,7 +170,8 @@ - + + diff --git a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md index 01cdc3756866b..03eaab2a3c6ca 100644 --- a/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md +++ b/perception/image_projection_based_fusion/docs/roi-cluster-fusion.md @@ -32,18 +32,20 @@ The clusters are projected onto image planes, and then if the ROIs of clusters a ### Core Parameters -| Name | Type | Description | -| --------------------------- | ----- | -------------------------------------------------------------------------------- | -| `use_iou_x` | bool | calculate IoU only along x-axis | -| `use_iou_y` | bool | calculate IoU only along y-axis | -| `use_iou` | bool | calculate IoU both along x-axis and y-axis | -| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | -| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | -| `roi_scale_factor` | float | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | -| `iou_threshold` | float | the IoU threshold to overwrite a label of clusters with a label of roi | -| `unknown_iou_threshold` | float | the IoU threshold to fuse cluster with unknown label of roi | -| `rois_number` | int | the number of input rois | -| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | +| Name | Type | Description | +| --------------------------- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `fusion_distance` | double | If the detected object's distance to frame_id is less than the threshold, the fusion will be processed | +| `trust_object_distance` | double | if the detected object's distance is less than the `trust_object_distance`, `trust_object_iou_mode` will be used, otherwise `non_trust_object_iou_mode` will be used | +| `trust_object_iou_mode` | string | select mode from 3 options {`iou`, `iou_x`, `iou_y`} to calculate IoU in range of [`0`, `trust_distance`].
 `iou`: IoU along x-axis and y-axis
 `iou_x`: IoU along x-axis
 `iou_y`: IoU along y-axis | +| `non_trust_object_iou_mode` | string | the IOU mode using in range of [`trust_distance`, `fusion_distance`] if `trust_distance` < `fusion_distance` | +| `use_cluster_semantic_type` | bool | if `false`, the labels of clusters are overwritten by `UNKNOWN` before fusion | +| `only_allow_inside_cluster` | bool | if `true`, the only clusters contained inside RoIs by a detector | +| `roi_scale_factor` | double | the scale factor for offset of detector RoIs if `only_allow_inside_cluster=true` | +| `iou_threshold` | double | the IoU threshold to overwrite a label of clusters with a label of roi | +| `unknown_iou_threshold` | double | the IoU threshold to fuse cluster with unknown label of roi | +| `remove_unknown` | bool | if `true`, remove all `UNKNOWN` labeled objects from output | +| `rois_number` | int | the number of input rois | +| `debug_mode` | bool | If `true`, subscribe and publish images for visualization. | ## Assumptions / Known limits diff --git a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp index 70a7866e79b87..e54710ad477da 100644 --- a/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp +++ b/perception/image_projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp @@ -17,10 +17,12 @@ #include "image_projection_based_fusion/fusion_node.hpp" +#include #include - +#include namespace image_projection_based_fusion { +const std::map IOU_MODE_MAP{{"iou", 0}, {"iou_x", 1}, {"iou_y", 2}}; class RoiClusterFusionNode : public FusionNode @@ -38,9 +40,7 @@ class RoiClusterFusionNode const sensor_msgs::msg::CameraInfo & camera_info, DetectedObjectsWithFeature & output_cluster_msg) override; - bool use_iou_x_{false}; - bool use_iou_y_{false}; - bool use_iou_{false}; + std::string trust_object_iou_mode_{"iou"}; bool use_cluster_semantic_type_{false}; bool only_allow_inside_cluster_{false}; double roi_scale_factor_{1.1}; @@ -49,10 +49,14 @@ class RoiClusterFusionNode const float min_roi_existence_prob_ = 0.1; // keep small value to lessen affect on merger object stage bool remove_unknown_; - double trust_distance_; - - bool filter_by_distance(const DetectedObjectWithFeature & obj); + double fusion_distance_; + double trust_object_distance_; + std::string non_trust_object_iou_mode_{"iou_x"}; + bool is_far_enough(const DetectedObjectWithFeature & obj, const double distance_threshold); bool out_of_scope(const DetectedObjectWithFeature & obj); + double cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode); // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; diff --git a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml index 2d99c25bb68f7..60f6f943b8cda 100644 --- a/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml +++ b/perception/image_projection_based_fusion/launch/roi_cluster_fusion.launch.xml @@ -1,5 +1,5 @@ - + @@ -20,7 +20,8 @@ - + + @@ -45,9 +46,8 @@ - - - + + @@ -55,7 +55,8 @@ - + + 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 316d29dae1301..4a46c8aace696 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 @@ -36,16 +36,16 @@ namespace image_projection_based_fusion RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) : FusionNode("roi_cluster_fusion", options) { - use_iou_x_ = declare_parameter("use_iou_x"); - use_iou_y_ = declare_parameter("use_iou_y"); - use_iou_ = declare_parameter("use_iou"); + trust_object_iou_mode_ = declare_parameter("trust_object_iou_mode"); + non_trust_object_iou_mode_ = declare_parameter("non_trust_object_iou_mode"); use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type"); only_allow_inside_cluster_ = declare_parameter("only_allow_inside_cluster"); roi_scale_factor_ = declare_parameter("roi_scale_factor"); iou_threshold_ = declare_parameter("iou_threshold"); unknown_iou_threshold_ = declare_parameter("unknown_iou_threshold"); remove_unknown_ = declare_parameter("remove_unknown"); - trust_distance_ = declare_parameter("trust_distance"); + fusion_distance_ = declare_parameter("fusion_distance"); + trust_object_distance_ = declare_parameter("trust_object_distance"); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -111,7 +111,7 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } - if (filter_by_distance(input_cluster_msg.feature_objects.at(i))) { + if (is_far_enough(input_cluster_msg.feature_objects.at(i), fusion_distance_)) { continue; } @@ -175,25 +175,23 @@ void RoiClusterFusionNode::fuseOnSingleImage( bool is_roi_label_known = feature_obj.object.classification.front().label != ObjectClassification::UNKNOWN; for (const auto & cluster_map : m_cluster_roi) { - double iou(0.0), iou_x(0.0), iou_y(0.0); - if (use_iou_) { - iou = calcIoU(cluster_map.second, feature_obj.feature.roi); - } - // use for unknown roi to improve small objects like traffic cone detect - // TODO(badai-nguyen): add option to disable roi_cluster mode - if (use_iou_x_ || !is_roi_label_known) { - iou_x = calcIoUX(cluster_map.second, feature_obj.feature.roi); - } - if (use_iou_y_) { - iou_y = calcIoUY(cluster_map.second, feature_obj.feature.roi); + 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_); + 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_); + } else { + iou = cal_iou_by_mode(cluster_map.second, feature_obj.feature.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; - if (max_iou < iou + iou_x + iou_y && passed_inside_cluster_gate) { + if (max_iou < iou && passed_inside_cluster_gate) { index = cluster_map.first; - max_iou = iou + iou_x + iou_y; + max_iou = iou; associated = true; } } @@ -274,11 +272,30 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) return is_out; } -bool RoiClusterFusionNode::filter_by_distance(const DetectedObjectWithFeature & obj) +bool RoiClusterFusionNode::is_far_enough( + const DetectedObjectWithFeature & obj, const double distance_threshold) { const auto & position = obj.object.kinematics.pose_with_covariance.pose.position; - const auto square_distance = position.x * position.x + position.y + position.y; - return square_distance > trust_distance_ * trust_distance_; + return position.x * position.x + position.y * position.y > + distance_threshold * distance_threshold; +} + +double RoiClusterFusionNode::cal_iou_by_mode( + const sensor_msgs::msg::RegionOfInterest & roi_1, + const sensor_msgs::msg::RegionOfInterest & roi_2, const std::string iou_mode) +{ + switch (IOU_MODE_MAP.at(iou_mode)) { + case 0 /* use iou mode */: + return calcIoU(roi_1, roi_2); + + case 1 /* use iou_x mode */: + return calcIoUX(roi_1, roi_2); + + case 2 /* use iou_y mode */: + return calcIoUY(roi_1, roi_2); + default: + return 0.0; + } } } // namespace image_projection_based_fusion