fix(shape_estimation): preserve irregular large size vehicle detected…

… by camera_lidar_fusion as unknown (autowarefoundation#6598) * fix(shape_estimation): keep excluded objects as unknown Signed-off-by: badai-nguyen <[email protected]> * chore: add optional param Signed-off-by: badai-nguyen <[email protected]> * chore: rename param Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]> Co-authored-by: Yoshi Ri <[email protected]> Signed-off-by: kaigohirao <[email protected]>
kaigohirao · Mar 22, 2024 · 2641a38 · 2641a38
1 parent 4545e1c
commit 2641a38
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 7 deletions.
diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/shape_estimation/config/shape_estimation.param.yaml
@@ -5,3 +5,4 @@
       use_vehicle_reference_yaw: false
       use_vehicle_reference_shape_size: false
       use_boost_bbox_optimizer: false
+      fix_filtered_objects_label_to_unknown: true
diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp
@@ -39,25 +39,31 @@ bool ShapeEstimator::estimateShapeAndPose(
   autoware_auto_perception_msgs::msg::Shape shape;
   geometry_msgs::msg::Pose pose;
   // estimate shape
+  bool reverse_to_unknown = false;
   if (!estimateOriginalShapeAndPose(label, cluster, ref_yaw_info, shape, pose)) {
-    return false;
+    reverse_to_unknown = true;
   }
 
   // rule based filter
   if (use_filter_) {
     if (!applyFilter(label, shape, pose)) {
-      return false;
+      reverse_to_unknown = true;
     }
   }
 
   // rule based corrector
   if (use_corrector_) {
     bool use_reference_yaw = ref_yaw_info ? true : false;
     if (!applyCorrector(label, use_reference_yaw, ref_shape_size_info, shape, pose)) {
-      return false;
+      reverse_to_unknown = true;
     }
   }
-
+  if (reverse_to_unknown) {
+    estimateOriginalShapeAndPose(Label::UNKNOWN, cluster, ref_yaw_info, shape, pose);
+    shape_output = shape;
+    pose_output = pose;
+    return false;
+  }
   shape_output = shape;
   pose_output = pose;
   return true;

diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp
@@ -47,6 +47,8 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option
   use_vehicle_reference_yaw_ = declare_parameter<bool>("use_vehicle_reference_yaw");
   use_vehicle_reference_shape_size_ = declare_parameter<bool>("use_vehicle_reference_shape_size");
   bool use_boost_bbox_optimizer = declare_parameter<bool>("use_boost_bbox_optimizer");
+  fix_filtered_objects_label_to_unknown_ =
+    declare_parameter<bool>("fix_filtered_objects_label_to_unknown");
   RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer);
   estimator_ =
     std::make_unique<ShapeEstimator>(use_corrector, use_filter, use_boost_bbox_optimizer);
@@ -103,12 +105,15 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared
     const bool estimated_success = estimator_->estimateShapeAndPose(
       label, *cluster, ref_yaw_info, ref_shape_size_info, shape, pose);
 
-    // If the shape estimation fails, ignore it.
-    if (!estimated_success) {
+    // If the shape estimation fails, change to Unknown object.
+    if (!fix_filtered_objects_label_to_unknown_ && !estimated_success) {
       continue;
     }
-
     output_msg.feature_objects.push_back(feature_object);
+    if (!estimated_success) {
+      output_msg.feature_objects.back().object.classification.front().label = Label::UNKNOWN;
+    }
+
     output_msg.feature_objects.back().object.shape = shape;
     output_msg.feature_objects.back().object.kinematics.pose_with_covariance.pose = pose;
   }

diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp
@@ -44,6 +44,7 @@ class ShapeEstimationNode : public rclcpp::Node
   std::unique_ptr<ShapeEstimator> estimator_;
   bool use_vehicle_reference_yaw_;
   bool use_vehicle_reference_shape_size_;
+  bool fix_filtered_objects_label_to_unknown_;
 
 public:
   explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options);