From c763eda68ede1ccc5b50290cb61f5775061aa212 Mon Sep 17 00:00:00 2001 From: badai nguyen <94814556+badai-nguyen@users.noreply.github.com> Date: Thu, 5 Oct 2023 11:32:43 +0900 Subject: [PATCH] fix(detected_object_validation): change the points_num of the validator to be set class by class (#5177) * fix: add param for each object class Signed-off-by: badai-nguyen * fix: add missing classes param Signed-off-by: badai-nguyen * fix: launch file Signed-off-by: badai-nguyen * fix: typo Signed-off-by: badai-nguyen * chore: refactor Signed-off-by: badai-nguyen --------- Signed-off-by: badai-nguyen --- ...mera_lidar_fusion_based_detection.launch.xml | 1 + ...idar_radar_fusion_based_detection.launch.xml | 1 + .../detection/lidar_based_detection.launch.xml | 1 + .../launch/perception.launch.xml | 1 + ...stacle_pointcloud_based_validator.param.yaml | 13 +++++++++++++ .../obstacle_pointcloud_based_validator.hpp | 8 ++++---- ...stacle_pointcloud_based_validator.launch.xml | 2 ++ .../src/obstacle_pointcloud_based_validator.cpp | 17 ++++++++++++----- 8 files changed, 35 insertions(+), 9 deletions(-) create mode 100644 perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 178e193dc2d99..51359d8d8f0e2 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -314,6 +314,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 38d141241c215..45cfb91bf25c1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -348,6 +348,7 @@ + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 3b6b9ba652a24..c844db39f4e9d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -115,6 +115,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index b0219376e9625..3d4ce1035e72c 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -17,6 +17,7 @@ + diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml new file mode 100644 index 0000000000000..745a0fd6591a9 --- /dev/null +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -0,0 +1,13 @@ +/**: + ros__parameters: + min_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + max_points_num: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [10, 10, 10, 10, 10, 10, 10, 10] + + min_points_and_distance_ratio: + #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN + [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 270e5c7bdb7ff..5819302664907 100644 --- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -32,14 +32,14 @@ #include #include - +#include namespace obstacle_pointcloud_based_validator { struct PointsNumThresholdParam { - size_t min_points_num; - size_t max_points_num; - float min_points_and_distance_ratio; + std::vector min_points_num; + std::vector max_points_num; + std::vector min_points_and_distance_ratio; }; class ObstaclePointCloudBasedValidator : public rclcpp::Node { diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml index 8196e95f37957..799b605658345 100644 --- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml +++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml @@ -3,11 +3,13 @@ + + diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index 00e53e9de9a9c..47640c9332e4d 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -92,10 +92,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - points_num_threshold_param_.min_points_num = declare_parameter("min_points_num", 10); - points_num_threshold_param_.max_points_num = declare_parameter("max_points_num", 10); + points_num_threshold_param_.min_points_num = + declare_parameter>("min_points_num"); + points_num_threshold_param_.max_points_num = + declare_parameter>("max_points_num"); points_num_threshold_param_.min_points_and_distance_ratio = - declare_parameter("min_points_and_distance_ratio", 800.0); + declare_parameter>("min_points_and_distance_ratio"); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); @@ -134,6 +136,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); + const auto object_label_id = transformed_object.classification.front().label; const auto & object = input_objects->objects.at(i); const auto & transformed_object_position = transformed_object.kinematics.pose_with_covariance.pose.position; @@ -155,13 +158,17 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud); // Filter object that have few pointcloud in them. + // TODO(badai-nguyen) add 3d validator option const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud); const auto object_distance = std::hypot(transformed_object_position.x, transformed_object_position.y); size_t min_pointcloud_num = std::clamp( static_cast( - points_num_threshold_param_.min_points_and_distance_ratio / object_distance + 0.5f), - points_num_threshold_param_.min_points_num, points_num_threshold_param_.max_points_num); + points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) / + object_distance + + 0.5f), + static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)), + static_cast(points_num_threshold_param_.max_points_num.at(object_label_id))); if (num) { (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) : removed_objects.objects.push_back(object);