From e9e97b9fa39e781f7bf57f12e7f9a755d9176b04 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 12 Mar 2024 15:27:53 +0900 Subject: [PATCH 1/3] fix(shape_estimation): keep excluded objects as unknown Signed-off-by: badai-nguyen --- .../shape_estimation/lib/shape_estimator.cpp | 14 ++++++++++---- perception/shape_estimation/src/node.cpp | 6 +++--- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/perception/shape_estimation/lib/shape_estimator.cpp b/perception/shape_estimation/lib/shape_estimator.cpp index e89eaac0a6db0..e0be98edf926a 100644 --- a/perception/shape_estimation/lib/shape_estimator.cpp +++ b/perception/shape_estimation/lib/shape_estimator.cpp @@ -39,14 +39,15 @@ 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; } } @@ -54,10 +55,15 @@ bool ShapeEstimator::estimateShapeAndPose( 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 index 22f31fffec21a..dc0a38b2746ab 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -103,12 +103,12 @@ 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 the shape estimation fails, change to Unknown object. + output_msg.feature_objects.push_back(feature_object); if (!estimated_success) { - continue; + output_msg.feature_objects.back().object.classification.front().label = Label::UNKNOWN; } - output_msg.feature_objects.push_back(feature_object); output_msg.feature_objects.back().object.shape = shape; output_msg.feature_objects.back().object.kinematics.pose_with_covariance.pose = pose; } From f73c162e3b862323bc607a0af1a6bdfa216233b9 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 13 Mar 2024 15:22:21 +0900 Subject: [PATCH 2/3] chore: add optional param Signed-off-by: badai-nguyen --- .../shape_estimation/config/shape_estimation.param.yaml | 1 + perception/shape_estimation/src/node.cpp | 4 ++++ perception/shape_estimation/src/node.hpp | 1 + 3 files changed, 6 insertions(+) diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/shape_estimation/config/shape_estimation.param.yaml index 253516fffe0d4..09204b7aeef13 100644 --- 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 + keep_supper_large_vehicle: true diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index dc0a38b2746ab..d6cc1d540ffed 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -47,6 +47,7 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw"); use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size"); bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer"); + keep_supper_large_vehicle_ = declare_parameter("keep_supper_large_vehicle"); RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); @@ -104,6 +105,9 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared label, *cluster, ref_yaw_info, ref_shape_size_info, shape, pose); // If the shape estimation fails, change to Unknown object. + if (!keep_supper_large_vehicle_ && !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; diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index 87df44f08f836..3c7b45287293a 100644 --- 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 estimator_; bool use_vehicle_reference_yaw_; bool use_vehicle_reference_shape_size_; + bool keep_supper_large_vehicle_; public: explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options); From e77d669b6547910c71040b4d93bd62c3f441fd3e Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Wed, 13 Mar 2024 16:16:32 +0900 Subject: [PATCH 3/3] chore: rename param Signed-off-by: badai-nguyen --- .../shape_estimation/config/shape_estimation.param.yaml | 2 +- perception/shape_estimation/src/node.cpp | 5 +++-- perception/shape_estimation/src/node.hpp | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/perception/shape_estimation/config/shape_estimation.param.yaml b/perception/shape_estimation/config/shape_estimation.param.yaml index 09204b7aeef13..e277d99d70edd 100644 --- a/perception/shape_estimation/config/shape_estimation.param.yaml +++ b/perception/shape_estimation/config/shape_estimation.param.yaml @@ -5,4 +5,4 @@ use_vehicle_reference_yaw: false use_vehicle_reference_shape_size: false use_boost_bbox_optimizer: false - keep_supper_large_vehicle: true + fix_filtered_objects_label_to_unknown: true diff --git a/perception/shape_estimation/src/node.cpp b/perception/shape_estimation/src/node.cpp index d6cc1d540ffed..423dda764492b 100644 --- a/perception/shape_estimation/src/node.cpp +++ b/perception/shape_estimation/src/node.cpp @@ -47,7 +47,8 @@ ShapeEstimationNode::ShapeEstimationNode(const rclcpp::NodeOptions & node_option use_vehicle_reference_yaw_ = declare_parameter("use_vehicle_reference_yaw"); use_vehicle_reference_shape_size_ = declare_parameter("use_vehicle_reference_shape_size"); bool use_boost_bbox_optimizer = declare_parameter("use_boost_bbox_optimizer"); - keep_supper_large_vehicle_ = declare_parameter("keep_supper_large_vehicle"); + fix_filtered_objects_label_to_unknown_ = + declare_parameter("fix_filtered_objects_label_to_unknown"); RCLCPP_INFO(this->get_logger(), "using boost shape estimation : %d", use_boost_bbox_optimizer); estimator_ = std::make_unique(use_corrector, use_filter, use_boost_bbox_optimizer); @@ -105,7 +106,7 @@ void ShapeEstimationNode::callback(const DetectedObjectsWithFeature::ConstShared label, *cluster, ref_yaw_info, ref_shape_size_info, shape, pose); // If the shape estimation fails, change to Unknown object. - if (!keep_supper_large_vehicle_ && !estimated_success) { + if (!fix_filtered_objects_label_to_unknown_ && !estimated_success) { continue; } output_msg.feature_objects.push_back(feature_object); diff --git a/perception/shape_estimation/src/node.hpp b/perception/shape_estimation/src/node.hpp index 3c7b45287293a..a1c30446605b0 100644 --- a/perception/shape_estimation/src/node.hpp +++ b/perception/shape_estimation/src/node.hpp @@ -44,7 +44,7 @@ class ShapeEstimationNode : public rclcpp::Node std::unique_ptr estimator_; bool use_vehicle_reference_yaw_; bool use_vehicle_reference_shape_size_; - bool keep_supper_large_vehicle_; + bool fix_filtered_objects_label_to_unknown_; public: explicit ShapeEstimationNode(const rclcpp::NodeOptions & node_options);