diff --git a/perception/autoware_lidar_transfusion/config/transfusion.param.yaml b/perception/autoware_lidar_transfusion/config/transfusion.param.yaml index 47eaa800e62fc..979f8b9cc09e3 100644 --- a/perception/autoware_lidar_transfusion/config/transfusion.param.yaml +++ b/perception/autoware_lidar_transfusion/config/transfusion.param.yaml @@ -10,7 +10,6 @@ densification_world_frame_id: map # post-process params circle_nms_dist_threshold: 0.5 - iou_nms_target_class_names: ["CAR"] iou_nms_search_distance_2d: 10.0 iou_nms_threshold: 0.1 yaw_norm_thresholds: [0.3, 0.3, 0.3, 0.3, 0.0] # refers to the class_names diff --git a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp index 9f07d2448fcae..26c4e324781d0 100644 --- a/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp +++ b/perception/autoware_lidar_transfusion/include/autoware/lidar_transfusion/postprocess/non_maximum_suppression.hpp @@ -28,35 +28,12 @@ namespace autoware::lidar_transfusion { using autoware_perception_msgs::msg::DetectedObject; -enum class NMS_TYPE { - IoU_BEV - // IoU_3D - // Distance_2D - // Distance_3D -}; - struct NMSParams { - NMS_TYPE nms_type_{}; - std::vector target_class_names_{}; double search_distance_2d_{}; double iou_threshold_{}; - // double distance_threshold_{}; }; -std::vector classNamesToBooleanMask(const std::vector & class_names) -{ - std::vector mask; - constexpr std::size_t num_object_classification = 8; - mask.resize(num_object_classification); - for (const auto & class_name : class_names) { - const auto semantic_type = getSemanticType(class_name); - mask.at(semantic_type) = true; - } - - return mask; -} - class NonMaximumSuppression { public: @@ -65,14 +42,13 @@ class NonMaximumSuppression std::vector apply(const std::vector &); private: - bool isTargetLabel(const std::uint8_t); - bool isTargetPairObject(const DetectedObject &, const DetectedObject &); Eigen::MatrixXd generateIoUMatrix(const std::vector &); NMSParams params_{}; - std::vector target_class_mask_{}; + + double search_distance_2d_sq_{}; }; } // namespace autoware::lidar_transfusion diff --git a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp index 1a288d1b0fd44..003358dc6b431 100644 --- a/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp +++ b/perception/autoware_lidar_transfusion/lib/postprocess/non_maximum_suppression.cpp @@ -22,6 +22,7 @@ namespace autoware::lidar_transfusion { +using Label = autoware_perception_msgs::msg::ObjectClassification; void NonMaximumSuppression::setParameters(const NMSParams & params) { @@ -29,15 +30,7 @@ void NonMaximumSuppression::setParameters(const NMSParams & params) assert(params.iou_threshold_ >= 0.0 && params.iou_threshold_ <= 1.0); params_ = params; - target_class_mask_ = classNamesToBooleanMask(params.target_class_names_); -} - -bool NonMaximumSuppression::isTargetLabel(const uint8_t label) -{ - if (label >= target_class_mask_.size()) { - return false; - } - return target_class_mask_.at(label); + search_distance_2d_sq_ = params.search_distance_2d_ * params.search_distance_2d_; } bool NonMaximumSuppression::isTargetPairObject( @@ -48,15 +41,15 @@ bool NonMaximumSuppression::isTargetPairObject( const auto label2 = autoware::object_recognition_utils::getHighestProbLabel(object2.classification); - if (isTargetLabel(label1) && isTargetLabel(label2)) { - return true; + // if labels are not the same, and one of them is pedestrian, do not suppress + if (label1 != label2 && (label1 == Label::PEDESTRIAN || label2 == Label::PEDESTRIAN)) { + return false; } - const auto search_sqr_dist_2d = params_.search_distance_2d_ * params_.search_distance_2d_; const auto sqr_dist_2d = autoware::universe_utils::calcSquaredDistance2d( autoware::object_recognition_utils::getPose(object1), autoware::object_recognition_utils::getPose(object2)); - return sqr_dist_2d <= search_sqr_dist_2d; + return sqr_dist_2d <= search_distance_2d_sq_; } Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( @@ -73,14 +66,12 @@ Eigen::MatrixXd NonMaximumSuppression::generateIoUMatrix( continue; } - if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { - const double iou = autoware::object_recognition_utils::get2dIoU(target_obj, source_obj); - triangular_matrix(target_i, source_i) = iou; - // NOTE: If the target object has any objects with iou > iou_threshold, it - // will be suppressed regardless of later results. - if (iou > params_.iou_threshold_) { - break; - } + const double iou = autoware::object_recognition_utils::get2dIoU(target_obj, source_obj); + triangular_matrix(target_i, source_i) = iou; + // NOTE: If the target object has any objects with iou > iou_threshold, it + // will be suppressed regardless of later results. + if (iou > params_.iou_threshold_) { + break; } } } @@ -97,10 +88,9 @@ std::vector NonMaximumSuppression::apply( output_objects.reserve(input_objects.size()); for (std::size_t i = 0; i < input_objects.size(); ++i) { const auto value = iou_matrix.row(i).maxCoeff(); - if (params_.nms_type_ == NMS_TYPE::IoU_BEV) { - if (value <= params_.iou_threshold_) { - output_objects.emplace_back(input_objects.at(i)); - } + + if (value <= params_.iou_threshold_) { + output_objects.emplace_back(input_objects.at(i)); } } diff --git a/perception/autoware_lidar_transfusion/schema/transfusion.schema.dummy.json b/perception/autoware_lidar_transfusion/schema/transfusion.schema.dummy.json index 6d9029f7e6806..739c641b7cefb 100644 --- a/perception/autoware_lidar_transfusion/schema/transfusion.schema.dummy.json +++ b/perception/autoware_lidar_transfusion/schema/transfusion.schema.dummy.json @@ -48,15 +48,6 @@ "default": 0.5, "minimum": 0.0 }, - "iou_nms_target_class_names": { - "type": "array", - "items": { - "type": "string" - }, - "description": "An array of class names to be target in NMS.", - "default": ["CAR"], - "uniqueItems": true - }, "iou_nms_search_distance_2d": { "type": "number", "description": "A maximum distance value to search the nearest objects.", @@ -95,7 +86,6 @@ "densification_num_past_frames", "densification_world_frame_id", "circle_nms_dist_threshold", - "iou_nms_target_class_names", "iou_nms_search_distance_2d", "iou_nms_threshold", "yaw_norm_thresholds", diff --git a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp index c75a5f434aeed..8bb70a821621f 100644 --- a/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp +++ b/perception/autoware_lidar_transfusion/src/lidar_transfusion_node.cpp @@ -65,9 +65,6 @@ LidarTransfusionNode::LidarTransfusionNode(const rclcpp::NodeOptions & options) static_cast(this->declare_parameter("circle_nms_dist_threshold", descriptor)); { // IoU NMS NMSParams p; - p.nms_type_ = NMS_TYPE::IoU_BEV; - p.target_class_names_ = - this->declare_parameter>("iou_nms_target_class_names", descriptor); p.search_distance_2d_ = this->declare_parameter("iou_nms_search_distance_2d", descriptor); p.iou_threshold_ = this->declare_parameter("iou_nms_threshold", descriptor); diff --git a/perception/autoware_lidar_transfusion/test/test_nms.cpp b/perception/autoware_lidar_transfusion/test/test_nms.cpp index 5449f61aa2284..dcb523abd96d8 100644 --- a/perception/autoware_lidar_transfusion/test/test_nms.cpp +++ b/perception/autoware_lidar_transfusion/test/test_nms.cpp @@ -24,8 +24,6 @@ TEST(NonMaximumSuppressionTest, Apply) autoware::lidar_transfusion::NMSParams params; params.search_distance_2d_ = 1.0; params.iou_threshold_ = 0.2; - params.nms_type_ = autoware::lidar_transfusion::NMS_TYPE::IoU_BEV; - params.target_class_names_ = {"CAR"}; nms.setParameters(params); std::vector input_objects(4);