fix(detected_object_validation): add 3d pointcloud based validator (#…

…5327) * fix: add 3d validation bind function Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: check validation point within shape Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * fix: add 2d validator option param Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: refactor Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: refactor Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * Revert "chore: refactor" This reverts commit e3cbf6c. * chore: update docs and param file Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * refactor: change to abstract class Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> * chore: add maintainer Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp> --------- Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>
autowarefoundation · Nov 15, 2023 · a76d360 · a76d360
1 parent b5d6c51
commit a76d360
Show file tree

Hide file tree

Showing 6 changed files with 344 additions and 138 deletions.
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
@@ -11,3 +11,5 @@
     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]
+
+    using_2d_validator: false
diff --git a/...ption/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp b/...ption/detected_object_validation/include/obstacle_pointcloud_based_validator/debugger.hpp
@@ -71,11 +71,18 @@ class Debugger
   void addNeighborPointcloud(const pcl::PointCloud<pcl::PointXY>::Ptr & input)
   {
     pcl::PointCloud<pcl::PointXYZ>::Ptr input_xyz = toXYZ(input);
-    for (const auto & point : *input_xyz) {
-      neighbor_pointcloud_->push_back(point);
-    }
+    addNeighborPointcloud(input_xyz);
   }
 
+  void addNeighborPointcloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr & input)
+  {
+    if (!input->empty()) {
+      neighbor_pointcloud_->reserve(neighbor_pointcloud_->size() + input->size());
+      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);

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
@@ -25,8 +25,13 @@
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/synchronizer.h>
+#include <pcl/filters/crop_hull.h>
+#include <pcl/kdtree/kdtree_flann.h>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <pcl/search/kdtree.h>
+#include <pcl/search/pcl_search.h>
+#include <pcl_conversions/pcl_conversions.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
@@ -35,12 +40,88 @@
 #include <vector>
 namespace obstacle_pointcloud_based_validator
 {
+
 struct PointsNumThresholdParam
 {
   std::vector<int64_t> min_points_num;
   std::vector<int64_t> max_points_num;
   std::vector<double> min_points_and_distance_ratio;
 };
+
+class Validator
+{
+private:
+  PointsNumThresholdParam points_num_threshold_param_;
+
+protected:
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_pointcloud_;
+
+public:
+  explicit Validator(PointsNumThresholdParam & points_num_threshold_param);
+  inline pcl::PointCloud<pcl::PointXYZ>::Ptr getDebugPointCloudWithinObject()
+  {
+    return cropped_pointcloud_;
+  }
+
+  virtual bool setKdtreeInputCloud(
+    const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_pointcloud) = 0;
+  virtual bool validate_object(
+    const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object) = 0;
+  virtual std::optional<float> getMaxRadius(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object) = 0;
+  size_t getThresholdPointCloud(const autoware_auto_perception_msgs::msg::DetectedObject & object);
+  virtual pcl::PointCloud<pcl::PointXYZ>::Ptr getDebugNeighborPointCloud() = 0;
+};
+
+class Validator2D : public Validator
+{
+private:
+  pcl::PointCloud<pcl::PointXY>::Ptr obstacle_pointcloud_;
+  pcl::PointCloud<pcl::PointXY>::Ptr neighbor_pointcloud_;
+  pcl::search::Search<pcl::PointXY>::Ptr kdtree_;
+
+public:
+  explicit Validator2D(PointsNumThresholdParam & points_num_threshold_param);
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr convertToXYZ(
+    const pcl::PointCloud<pcl::PointXY>::Ptr & pointcloud_xy);
+  inline pcl::PointCloud<pcl::PointXYZ>::Ptr getDebugNeighborPointCloud()
+  {
+    return convertToXYZ(neighbor_pointcloud_);
+  }
+
+  bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud);
+  bool validate_object(
+    const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object);
+  std::optional<float> getMaxRadius(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object);
+  std::optional<size_t> getPointCloudWithinObject(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const pcl::PointCloud<pcl::PointXY>::Ptr neighbor_pointcloud);
+};
+class Validator3D : public Validator
+{
+private:
+  pcl::PointCloud<pcl::PointXYZ>::Ptr obstacle_pointcloud_;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr neighbor_pointcloud_;
+  pcl::search::Search<pcl::PointXYZ>::Ptr kdtree_;
+
+public:
+  explicit Validator3D(PointsNumThresholdParam & points_num_threshold_param);
+  inline pcl::PointCloud<pcl::PointXYZ>::Ptr getDebugNeighborPointCloud()
+  {
+    return neighbor_pointcloud_;
+  }
+  bool setKdtreeInputCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud);
+  bool validate_object(
+    const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object);
+  std::optional<float> getMaxRadius(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object);
+  std::optional<size_t> getPointCloudWithinObject(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const pcl::PointCloud<pcl::PointXYZ>::Ptr neighbor_pointcloud);
+};
+
 class ObstaclePointCloudBasedValidator : public rclcpp::Node
 {
 public:
@@ -61,16 +142,13 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
   PointsNumThresholdParam points_num_threshold_param_;
 
   std::shared_ptr<Debugger> debugger_;
+  bool using_2d_validator_;
+  std::unique_ptr<Validator> validator_;
 
 private:
   void onObjectsAndObstaclePointCloud(
     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<float> getMaxRadius(
-    const autoware_auto_perception_msgs::msg::DetectedObject & object);
 };
 }  // namespace obstacle_pointcloud_based_validator
 

diff --git a/perception/detected_object_validation/obstacle-pointcloud-based-validator.md b/perception/detected_object_validation/obstacle-pointcloud-based-validator.md
@@ -28,6 +28,7 @@ In the debug image above, the red DetectedObject is the validated object. The bl
 
 | Name                            | Type  | Description                                                                                                                                                                |
 | ------------------------------- | ----- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| `using_2d_validator`            | bool  | The xy-plane projected (2D) obstacle point clouds will be used for validation                                                                                              |
 | `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. |

diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml
@@ -6,6 +6,7 @@
   <description>The ROS 2 detected_object_validation package</description>
   <maintainer email="yukihiro.saito@tier4.jp">Yukihiro Saito</maintainer>
   <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
+  <maintainer email="dai.nguyen@tier4.jp">badai nguyen</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>