fix(detected_object_validation): change the points_num of the validat…

…or to be set class by class (#5177) * fix: add param for each object class Signed-off-by: badai-nguyen <[email protected]> * fix: add missing classes param Signed-off-by: badai-nguyen <[email protected]> * fix: launch file Signed-off-by: badai-nguyen <[email protected]> * fix: typo Signed-off-by: badai-nguyen <[email protected]> * chore: refactor Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]>
autowarefoundation · Oct 5, 2023 · c763eda · c763eda
1 parent 8649d93
commit c763eda
Show file tree

Hide file tree

Showing 8 changed files with 35 additions and 9 deletions.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -314,6 +314,7 @@
       <arg name="input/detected_objects" value="$(var lidar_detection_model)/roi_fusion/objects"/>
       <arg name="input/obstacle_pointcloud" value="$(var validator/input/obstacle_pointcloud)"/>
       <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
+      <arg name="obstacle_pointcloud_based_validator_param_path" value="$(var object_recognition_detection_obstacle_pointcloud_based_validator_param_path)"/>
     </include>
   </group>
 

diff --git a/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -348,6 +348,7 @@
       <arg name="input/detected_objects" value="$(var lidar_detection_model)/roi_fusion/objects"/>
       <arg name="input/obstacle_pointcloud" value="$(var validator/input/obstacle_pointcloud)"/>
       <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
+      <arg name="obstacle_pointcloud_based_validator_param_path" value="$(var object_recognition_detection_obstacle_pointcloud_based_validator_param_path)"/>
     </include>
   </group>
 

diff --git a/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -115,6 +115,7 @@
       <arg name="input/detected_objects" value="$(var lidar_detection_model)/objects"/>
       <arg name="input/obstacle_pointcloud" value="$(var validator/input/obstacle_pointcloud)"/>
       <arg name="output/objects" value="$(var lidar_detection_model)/validation/objects"/>
+      <arg name="obstacle_pointcloud_based_validator_param_path" value="$(var object_recognition_detection_obstacle_pointcloud_based_validator_param_path)"/>
     </include>
   </group>
 

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -17,6 +17,7 @@
   <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
+  <arg name="object_recognition_detection_obstacle_pointcloud_based_validator_param_path"/>
   <arg name="occupancy_grid_map_method"/>
   <arg name="occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>

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
@@ -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/...ation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/...ation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
@@ -32,14 +32,14 @@
 
 #include <memory>
 #include <optional>
-
+#include <vector>
 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<int64_t> min_points_num;
+  std::vector<int64_t> max_points_num;
+  std::vector<double> 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
@@ -3,11 +3,13 @@
   <arg name="input/detected_objects" default="/perception/object_recognition/detection/objects"/>
   <arg name="input/obstacle_pointcloud" default="/perception/obstacle_segmentation/pointcloud"/>
   <arg name="output/objects" default="/perception/object_recognition/detection/validation/obstacle_pointcloud_based/objects"/>
+  <arg name="obstacle_pointcloud_based_validator_param_path" default="$(find-pkg-share detected_object_validation)/config/obstacle_pointcloud_based_validator.param.yaml"/>
 
   <node pkg="detected_object_validation" exec="obstacle_pointcloud_based_validator_node" name="obstacle_pointcloud_based_validator_node" output="screen">
     <remap from="~/input/detected_objects" to="$(var input/detected_objects)"/>
     <remap from="~/input/obstacle_pointcloud" to="$(var input/obstacle_pointcloud)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
     <param name="enable_debugger" value="false"/>
+    <param from="$(var obstacle_pointcloud_based_validator_param_path)"/>
   </node>
 </launch>
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,10 +92,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
   objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "~/output/objects", rclcpp::QoS{1});
 
-  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_num =
+    declare_parameter<std::vector<int64_t>>("min_points_num");
+  points_num_threshold_param_.max_points_num =
+    declare_parameter<std::vector<int64_t>>("max_points_num");
   points_num_threshold_param_.min_points_and_distance_ratio =
-    declare_parameter<float>("min_points_and_distance_ratio", 800.0);
+    declare_parameter<std::vector<double>>("min_points_and_distance_ratio");
 
   const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
   if (enable_debugger) debugger_ = std::make_shared<Debugger>(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<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);
+        points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) /
+          object_distance +
+        0.5f),
+      static_cast<size_t>(points_num_threshold_param_.min_points_num.at(object_label_id)),
+      static_cast<size_t>(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);