fix: add 3d validation bind function

Signed-off-by: badai-nguyen <[email protected]>

perception/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp

@@ -76,6 +76,12 @@ class Debugger
     }
   }
 
+  void addNeighborPointcloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr & input)
+  {
+    for (const auto & point : *input) {
+      neighbor_pointcloud_->push_back(point);
+    }
+  }
   void addPointcloudWithinPolygon(const pcl::PointCloud<pcl::PointXYZ>::Ptr & input)
   {
     // pcl::PointCloud<pcl::PointXYZ>::Ptr input_xyz = toXYZ(input);

perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp

@@ -61,14 +61,21 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
   PointsNumThresholdParam points_num_threshold_param_;
 
   std::shared_ptr<Debugger> debugger_;
+  bool using_2d_validator_;
 
 private:
   void onObjectsAndObstaclePointCloud(
     const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
     const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud);
+  void on3dObjectsAndObstaclePointCloud(
+    const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud);
   std::optional<size_t> getPointCloudNumWithinPolygon(
     const autoware_auto_perception_msgs::msg::DetectedObject & object,
     const pcl::PointCloud<pcl::PointXY>::Ptr pointcloud);
+  std::optional<size_t> getPointCloudNumWithinShape(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud);
   std::optional<float> getMaxRadius(
     const autoware_auto_perception_msgs::msg::DetectedObject & object);
 };

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -85,24 +85,108 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
   tf_listener_(tf_buffer_),
   sync_(SyncPolicy(10), objects_sub_, obstacle_pointcloud_sub_)
 {
-  using std::placeholders::_1;
-  using std::placeholders::_2;
-  sync_.registerCallback(
-    std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2));
-  objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
-    "~/output/objects", rclcpp::QoS{1});
-
   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<std::vector<double>>("min_points_and_distance_ratio");
 
+  using_2d_validator_ = declare_parameter<bool>("using_2d_validator");
+
+  using std::placeholders::_1;
+  using std::placeholders::_2;
+
+  if (using_2d_validator_) {
+    sync_.registerCallback(
+      std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2));
+  } else {
+    sync_.registerCallback(
+      std::bind(&ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud, this, _1, _2));
+  }
+
+  objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
+    "~/output/objects", rclcpp::QoS{1});
+
   const bool enable_debugger = declare_parameter<bool>("enable_debugger", false);
   if (enable_debugger) debugger_ = std::make_shared<Debugger>(this);
 }
+void ObstaclePointCloudBasedValidator::on3dObjectsAndObstaclePointCloud(
+  const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud)
+{
+  autoware_auto_perception_msgs::msg::DetectedObjects output, removed_objects;
+  output.header = input_objects->header;
+  removed_objects.header = input_objects->header;
+
+  // Transform to pointcloud frame
+  autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
+  if (!object_recognition_utils::transformObjects(
+        *input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_,
+        transformed_objects)) {
+    return;
+  }
+
+  // Convert to PCL
+  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud);
+  if (obstacle_pointcloud->empty()) {
+    return;
+  }
 
+  // Create Kd-tree to search neighbor pointcloud to reduce cost
+  pcl::search::Search<pcl::PointXYZ>::Ptr kdtree =
+    pcl::make_shared<pcl::search::KdTree<pcl::PointXYZ>>(false);
+  kdtree->setInputCloud(obstacle_pointcloud);
+  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;
+    const auto search_radius = getMaxRadius(transformed_object);
+    if (!search_radius) {
+      output.objects.push_back(object);
+      continue;
+    }
+    // Search neighbor pointcloud to reduce cost
+    pcl::PointCloud<pcl::PointXYZ>::Ptr neighbor_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
+    std::vector<int> indices;
+    std::vector<float> distances;
+    kdtree->radiusSearch(
+      pcl::PointXYZ(
+        transformed_object_position.x, transformed_object_position.y,
+        transformed_object_position.z),
+      search_radius.value(), indices, distances);
+    for (const auto & indice : indices) {
+      neighbor_pointcloud->push_back(obstacle_pointcloud->at(indice));
+    }
+
+    if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud);
+    // Filter object that have few pointcloud in them
+    const auto num = getPointCloudNumWithinShape(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.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);
+    } else {
+      output.objects.push_back(object);
+    }
+    objects_pub_->publish(output);
+    if (debugger_) {
+      debugger_->publishRemovedObjects(removed_objects);
+      debugger_->publishNeighborPointcloud(input_obstacle_pointcloud->header);
+    }
+  }
+}
 void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud)
@@ -184,6 +268,41 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
     debugger_->publishPointcloudWithinPolygon(input_obstacle_pointcloud->header);
   }
 }
+std::optional<size_t> ObstaclePointCloudBasedValidator::getPointCloudNumWithinShape(
+  const autoware_auto_perception_msgs::msg::DetectedObject & object,
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud)
+{
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
+  std::vector<pcl::Vertices> vertices_array;
+  pcl::Vertices vertices;
+
+  auto const & object_position = object.kinematics.pose_with_covariance.pose.position;
+  auto const object_height = object.shape.dimensions.z;
+  float z_min = object_position.z - object_height / 2.0f;
+  float z_max = object_position.z + object_height / 2.0f;
+
+  Polygon2d poly2d =
+    tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape);
+  if (bg::is_empty(poly2d)) return std::nullopt;
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr poly3d(new pcl::PointCloud<pcl::PointXYZ>);
+
+  for (size_t i = 0; i < poly2d.outer().size(); ++i) {
+    vertices.vertices.emplace_back(i);
+    vertices_array.emplace_back(vertices);
+    poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), z_min);
+    poly3d->emplace_back(poly2d.outer().at(i).x(), poly2d.outer().at(i).y(), z_max);
+  }
+  pcl::CropHull<pcl::PointXYZ> cropper;
+  cropper.setInputCloud(pointcloud);
+  cropper.setDim(3);
+  cropper.setHullIndices(vertices_array);
+  cropper.setHullCloud(poly3d);
+  cropper.setCropOutside(true);
+  cropper.filter(*cropped_pointcloud);
+  if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud);
+  return cropped_pointcloud->size();
+}
 
 std::optional<size_t> ObstaclePointCloudBasedValidator::getPointCloudNumWithinPolygon(
   const autoware_auto_perception_msgs::msg::DetectedObject & object,