feat(detected_object_validation): add dynamic min pointcloud num (aut…

…owarefoundation#2894) * feat: add dynamic min pointcloud num Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> * Update perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp * update readme Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com> --------- Signed-off-by: Yukihiro Saito <yukky.saito@gmail.com>
1222-takeshi · Mar 6, 2023 · 54942fe · 54942fe
1 parent 61a7eee
commit 54942fe
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 10 deletions.
diff --git a/...ation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/...ation/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<SyncPolicy> Sync;
   Sync sync_;
-  size_t min_pointcloud_num_;
+  PointsNumThresholdParam points_num_threshold_param_;
 
   std::shared_ptr<Debugger> debugger_;
 

diff --git 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
@@ -92,7 +92,10 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
   objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "~/output/objects", rclcpp::QoS{1});
 
-  min_pointcloud_num_ = declare_parameter<int>("min_pointcloud_num", 10);
+  points_num_threshold_param_.min_points_num = declare_parameter<int>("min_points_num", 10);
+  points_num_threshold_param_.max_points_num = declare_parameter<int>("max_points_num", 10);
+  points_num_threshold_param_.min_points_and_distance_ratio =
+    declare_parameter<float>("min_points_and_distance_ratio", 800.0);
 
   const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
   if (enable_debugger) debugger_ = std::make_shared<Debugger>(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,18 +148,23 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
     std::vector<int> indices;
     std::vector<float> 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));
     }
     if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud);
 
     // 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<size_t>(
+        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);
     }