From 1f0e5184d6aafed1e074d17b6beddd5cb41e0822 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Fri, 20 Jan 2023 14:28:48 +0900 Subject: [PATCH 1/2] fix(roi_cluster_fusion): add unknown-object removal (#2701) Signed-off-by: badai-nguyen Signed-off-by: badai-nguyen --- .../roi_cluster_fusion/node.hpp | 3 +++ .../launch/roi_cluster_fusion.launch.xml | 2 ++ .../src/roi_cluster_fusion/node.cpp | 18 ++++++++++++++++++ 3 files changed, 23 insertions(+) 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 8832e5ee1a52d..303d2acd9801f 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 @@ -30,6 +30,7 @@ class RoiClusterFusionNode protected: void preprocess(DetectedObjectsWithFeature & output_cluster_msg) override; + void postprocess(DetectedObjectsWithFeature & output_cluster_msg) override; void fuseOnSingleImage( const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, @@ -42,8 +43,10 @@ class RoiClusterFusionNode bool use_iou_{false}; bool use_cluster_semantic_type_{false}; float iou_threshold_{0.0f}; + bool remove_unknown_; bool out_of_scope(const DetectedObjectWithFeature & obj); + // bool CheckUnknown(const DetectedObjectsWithFeature & obj); }; } // namespace image_projection_based_fusion 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 1ba46fafe3729..63c4526cc04de 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 @@ -19,6 +19,7 @@ + @@ -74,6 +75,7 @@ + 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 295326f9ba976..fc3e714e8c91f 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 @@ -39,6 +39,7 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) use_iou_ = declare_parameter("use_iou", false); use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); iou_threshold_ = declare_parameter("iou_threshold", 0.1); + remove_unknown_ = declare_parameter("remove_unknown", false); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -53,6 +54,23 @@ void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluste } } +void RoiClusterFusionNode::postprocess(DetectedObjectsWithFeature & output_cluster_msg) +{ + if (!remove_unknown_) { + return; + } + DetectedObjectsWithFeature known_objects; + known_objects.feature_objects.reserve(output_cluster_msg.feature_objects.size()); + for (auto & feature_object : output_cluster_msg.feature_objects) { + if ( + feature_object.object.classification.front().label != + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN) { + known_objects.feature_objects.push_back(feature_object); + } + } + output_cluster_msg.feature_objects = known_objects.feature_objects; +} + void RoiClusterFusionNode::fuseOnSingleImage( const DetectedObjectsWithFeature & input_cluster_msg, const std::size_t image_id, const DetectedObjectsWithFeature & input_roi_msg, From ecf875facbdb1be1dcef9a247eb1f3782d9aea46 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kaan=20=C3=87olak?= Date: Sat, 11 Feb 2023 12:19:22 +0300 Subject: [PATCH 2/2] feat(image_projection_based_fusion): add trust distance to fusion (#2844) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(image_projection_based_fusion): add thrust distance to fusion Signed-off-by: Kaan Çolak * feat(image_projection_based_fusion): fix typo Signed-off-by: Kaan Çolak --------- Signed-off-by: Kaan Çolak --- .../roi_cluster_fusion/node.hpp | 2 ++ .../launch/roi_cluster_fusion.launch.xml | 2 ++ .../src/roi_cluster_fusion/node.cpp | 12 ++++++++++++ 3 files changed, 16 insertions(+) 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 303d2acd9801f..5d6aba9b7d51d 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 @@ -44,7 +44,9 @@ class RoiClusterFusionNode bool use_cluster_semantic_type_{false}; float iou_threshold_{0.0f}; bool remove_unknown_; + float trust_distance_; + bool filter_by_distance(const DetectedObjectWithFeature & obj); bool out_of_scope(const DetectedObjectWithFeature & obj); // 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 63c4526cc04de..298534c0d9ce3 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 @@ -20,6 +20,7 @@ + @@ -76,6 +77,7 @@ + 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 fc3e714e8c91f..717f21da21fc3 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 @@ -40,6 +40,7 @@ RoiClusterFusionNode::RoiClusterFusionNode(const rclcpp::NodeOptions & options) use_cluster_semantic_type_ = declare_parameter("use_cluster_semantic_type", false); iou_threshold_ = declare_parameter("iou_threshold", 0.1); remove_unknown_ = declare_parameter("remove_unknown", false); + trust_distance_ = declare_parameter("trust_distance", 100.0); } void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg) @@ -103,6 +104,10 @@ void RoiClusterFusionNode::fuseOnSingleImage( continue; } + if (filter_by_distance(input_cluster_msg.feature_objects.at(i))) { + continue; + } + // filter point out of scope if (debugger_ && out_of_scope(input_cluster_msg.feature_objects.at(i))) { continue; @@ -225,6 +230,13 @@ bool RoiClusterFusionNode::out_of_scope(const DetectedObjectWithFeature & obj) return is_out; } +bool RoiClusterFusionNode::filter_by_distance(const DetectedObjectWithFeature & obj) +{ + 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_; +} + } // namespace image_projection_based_fusion #include