Merge pull request autowarefoundation#318 from tier4/feat/image-proje…

…ction-based-fusion * fix(roi_cluster_fusion): add unknown-object removal (autowarefoundation#2701) Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * feat(image_projection_based_fusion): add trust distance to fusion (autowarefoundation#2844) * feat(image_projection_based_fusion): add thrust distance to fusion Signed-off-by: Kaan Çolak <kaancolak95@gmail.com> * feat(image_projection_based_fusion): fix typo Signed-off-by: Kaan Çolak <kaancolak95@gmail.com> --------- Signed-off-by: Kaan Çolak <kaancolak95@gmail.com> --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> Signed-off-by: Kaan Çolak <kaancolak95@gmail.com> Co-authored-by: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Co-authored-by: Kaan Çolak <kaancolak95@gmail.com>
shmpwk · Mar 15, 2023 · 4dfb3fa · 4dfb3fa
2 parents 13083d2 + ecf875f
commit 4dfb3fa
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 0 deletions.
diff --git a/...projection_based_fusion/include/image_projection_based_fusion/roi_cluster_fusion/node.hpp b/...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,12 @@ class RoiClusterFusionNode
   bool use_iou_{false};
   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);
 };
 
 }  // 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
@@ -19,6 +19,8 @@
   <arg name="input/clusters" default="clusters"/>
   <arg name="output/clusters" default="labeled_clusters"/>
   <arg name="sync_param_path" default="$(find-pkg-share image_projection_based_fusion)/config/roi_sync.param.yaml"/>
+  <arg name="remove_unknown" default="true"/>
+  <arg name="trust_distance" default="100.0"/>
 
   <!-- for eval variable-->
   <arg name="input_rois_number" default="$(var input/rois_number)"/>
@@ -74,6 +76,8 @@
       <param name="use_iou_y" value="false"/>
       <param name="iou_threshold" value="0.35"/>
       <param name="rois_number" value="$(var input/rois_number)"/>
+      <param name="remove_unknown" value="$(var remove_unknown)"/>
+      <param name="trust_distance" value="$(var trust_distance)"/>
       <param from="$(var sync_param_path)"/>
       <remap from="input" to="$(var input/clusters)"/>
       <remap from="output" to="$(var output/clusters)"/>

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
@@ -39,6 +39,8 @@ 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);
+  trust_distance_ = declare_parameter("trust_distance", 100.0);
 }
 
 void RoiClusterFusionNode::preprocess(DetectedObjectsWithFeature & output_cluster_msg)
@@ -53,6 +55,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,
@@ -85,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;
@@ -207,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 <rclcpp_components/register_node_macro.hpp>