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 fb89a7f4a4b15..270e5c7bdb7ff 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 @@ -35,6 +35,12 @@ namespace obstacle_pointcloud_based_validator { +struct PointsNumThresholdParam +{ + size_t min_points_num; + size_t max_points_num; + float min_points_and_distance_ratio; +}; class ObstaclePointCloudBasedValidator : public rclcpp::Node { public: @@ -52,7 +58,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node SyncPolicy; typedef message_filters::Synchronizer Sync; Sync sync_; - size_t min_pointcloud_num_; + PointsNumThresholdParam points_num_threshold_param_; std::shared_ptr debugger_; diff --git a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md index 3664f830c8b59..dd67aab3db0c9 100644 --- a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md +++ b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md @@ -26,10 +26,12 @@ In the debug image above, the red DetectedObject is the validated object. The bl ## Parameters -| Name | Type | Description | -| -------------------- | ----- | ---------------------------------------------------------------------------- | -| `min_pointcloud_num` | float | Threshold for the minimum number of obstacle point clouds in DetectedObjects | -| `enable_debugger` | bool | Whether to create debug topics or not? | +| Name | Type | Description | +| ------------------------------- | ----- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `min_points_num` | int | The minimum number of obstacle point clouds in DetectedObjects | +| `max_points_num` | int | The max number of obstacle point clouds in DetectedObjects | +| `min_points_and_distance_ratio` | float | Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink. | +| `enable_debugger` | bool | Whether to create debug topics or not? | ## Assumptions / Known limits 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 52e7c84f005fb..9903ce857b8aa 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -92,7 +92,10 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( objects_pub_ = create_publisher( "~/output/objects", rclcpp::QoS{1}); - min_pointcloud_num_ = declare_parameter("min_pointcloud_num", 10); + 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_and_distance_ratio = + declare_parameter("min_points_and_distance_ratio", 800.0); const bool enable_debugger = declare_parameter("enable_debugger", false); if (enable_debugger) debugger_ = std::make_shared(this); @@ -132,6 +135,8 @@ 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 = input_objects->objects.at(i); + const auto & transformed_object_position = + transformed_object.kinematics.pose_with_covariance.pose.position; const auto search_radius = getMaxRadius(transformed_object); if (!search_radius) { output.objects.push_back(object); @@ -143,8 +148,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( std::vector indices; std::vector distances; kdtree->radiusSearch( - toPCL(transformed_object.kinematics.pose_with_covariance.pose.position), - search_radius.value(), indices, distances); + toPCL(transformed_object_position), search_radius.value(), indices, distances); for (const auto & index : indices) { neighbor_pointcloud->push_back(obstacle_pointcloud->at(index)); } @@ -152,9 +156,15 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( // Filter object that have few pointcloud in them. 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); if (num) { - (min_pointcloud_num_ <= num.value()) ? output.objects.push_back(object) - : removed_objects.objects.push_back(object); + (min_pointcloud_num <= num.value()) ? output.objects.push_back(object) + : removed_objects.objects.push_back(object); } else { output.objects.push_back(object); }