refactor: change to abstract class

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

perception/detected_object_validation/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:
@@ -62,24 +143,12 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
 
   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);
-  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> getMaxRadius2D(
-    const autoware_auto_perception_msgs::msg::DetectedObject & object);
-  std::optional<float> getMaxRadius3D(
-    const autoware_auto_perception_msgs::msg::DetectedObject & object);
 };
 }  // namespace obstacle_pointcloud_based_validator