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>

Author: badai-nguyen

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

perception/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);

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:
@@ -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
 

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. |

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>

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -19,12 +19,6 @@
 
 #include <boost/geometry.hpp>
 
-#include <pcl/filters/crop_hull.h>
-#include <pcl/kdtree/kdtree_flann.h>
-#include <pcl/search/kdtree.h>
-#include <pcl/search/pcl_search.h>
-#include <pcl_conversions/pcl_conversions.h>
-
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
@@ -35,44 +29,247 @@
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 
-namespace
+namespace obstacle_pointcloud_based_validator
 {
-inline pcl::PointXY toPCL(const double x, const double y)
+namespace bg = boost::geometry;
+using Shape = autoware_auto_perception_msgs::msg::Shape;
+using Polygon2d = tier4_autoware_utils::Polygon2d;
+
+Validator::Validator(PointsNumThresholdParam & points_num_threshold_param)
 {
-  pcl::PointXY pcl_point;
-  pcl_point.x = x;
-  pcl_point.y = y;
-  return pcl_point;
+  points_num_threshold_param_.min_points_num = points_num_threshold_param.min_points_num;
+  points_num_threshold_param_.max_points_num = points_num_threshold_param.max_points_num;
+  points_num_threshold_param_.min_points_and_distance_ratio =
+    points_num_threshold_param.min_points_and_distance_ratio;
 }
 
-inline pcl::PointXY toPCL(const geometry_msgs::msg::Point & point)
+size_t Validator::getThresholdPointCloud(
+  const autoware_auto_perception_msgs::msg::DetectedObject & object)
 {
-  return toPCL(point.x, point.y);
+  const auto object_label_id = object.classification.front().label;
+  const auto object_distance = std::hypot(
+    object.kinematics.pose_with_covariance.pose.position.x,
+    object.kinematics.pose_with_covariance.pose.position.y);
+  size_t threshold_pc = 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)));
+  return threshold_pc;
 }
 
-inline pcl::PointXYZ toXYZ(const pcl::PointXY & point)
+Validator2D::Validator2D(PointsNumThresholdParam & points_num_threshold_param)
+: Validator(points_num_threshold_param)
 {
-  return pcl::PointXYZ(point.x, point.y, 0.0);
 }
 
-inline pcl::PointCloud<pcl::PointXYZ>::Ptr toXYZ(
-  const pcl::PointCloud<pcl::PointXY>::Ptr & pointcloud)
+bool Validator2D::setKdtreeInputCloud(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud)
+{
+  obstacle_pointcloud_.reset(new pcl::PointCloud<pcl::PointXY>);
+  pcl::fromROSMsg(*input_cloud, *obstacle_pointcloud_);
+  if (obstacle_pointcloud_->empty()) {
+    return false;
+  }
+
+  kdtree_ = pcl::make_shared<pcl::search::KdTree<pcl::PointXY>>(false);
+  kdtree_->setInputCloud(obstacle_pointcloud_);
+
+  return true;
+}
+pcl::PointCloud<pcl::PointXYZ>::Ptr Validator2D::convertToXYZ(
+  const pcl::PointCloud<pcl::PointXY>::Ptr & pointcloud_xy)
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud_xyz(new pcl::PointCloud<pcl::PointXYZ>);
-  pointcloud_xyz->reserve(pointcloud->size());
-  for (const auto & point : *pointcloud) {
-    pointcloud_xyz->push_back(toXYZ(point));
+  pointcloud_xyz->reserve(pointcloud_xy->size());
+  for (const auto & point : *pointcloud_xy) {
+    pointcloud_xyz->push_back(pcl::PointXYZ(point.x, point.y, 0.0));
   }
   return pointcloud_xyz;
 }
 
-}  // namespace
+std::optional<size_t> Validator2D::getPointCloudWithinObject(
+  const autoware_auto_perception_msgs::msg::DetectedObject & object,
+  const pcl::PointCloud<pcl::PointXY>::Ptr pointcloud)
+{
+  std::vector<pcl::Vertices> vertices_array;
+  pcl::Vertices vertices;
+  Polygon2d poly2d =
+    tier4_autoware_utils::toPolygon2d(object.kinematics.pose_with_covariance.pose, object.shape);
+  if (bg::is_empty(poly2d)) return std::nullopt;
 
-namespace obstacle_pointcloud_based_validator
+  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(), 0.0);
+  }
+  cropped_pointcloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::CropHull<pcl::PointXYZ> cropper;  // don't be implemented PointXY by PCL
+  cropper.setInputCloud(convertToXYZ(pointcloud));
+  cropper.setDim(2);
+  cropper.setHullIndices(vertices_array);
+  cropper.setHullCloud(poly3d);
+  cropper.setCropOutside(true);
+  cropper.filter(*cropped_pointcloud_);
+  return cropped_pointcloud_->size();
+}
+
+bool Validator2D::validate_object(
+  const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object)
 {
-namespace bg = boost::geometry;
-using Shape = autoware_auto_perception_msgs::msg::Shape;
-using Polygon2d = tier4_autoware_utils::Polygon2d;
+  // get neighbor_pointcloud of object
+  neighbor_pointcloud_.reset(new pcl::PointCloud<pcl::PointXY>);
+  std::vector<int> indices;
+  std::vector<float> distances;
+  const auto search_radius = getMaxRadius(transformed_object);
+  if (!search_radius) {
+    return false;
+  }
+  kdtree_->radiusSearch(
+    pcl::PointXY(
+      transformed_object.kinematics.pose_with_covariance.pose.position.x,
+      transformed_object.kinematics.pose_with_covariance.pose.position.y),
+    search_radius.value(), indices, distances);
+  for (const auto & index : indices) {
+    neighbor_pointcloud_->push_back(obstacle_pointcloud_->at(index));
+  }
+  const auto num = getPointCloudWithinObject(transformed_object, neighbor_pointcloud_);
+  if (!num) return true;
+
+  size_t threshold_pointcloud_num = getThresholdPointCloud(transformed_object);
+  if (num.value() > threshold_pointcloud_num) {
+    return true;
+  }
+  return false;  // remove object
+}
+
+std::optional<float> Validator2D::getMaxRadius(
+  const autoware_auto_perception_msgs::msg::DetectedObject & object)
+{
+  if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) {
+    return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
+  } else if (object.shape.type == Shape::POLYGON) {
+    float max_dist = 0.0;
+    for (const auto & point : object.shape.footprint.points) {
+      const float dist = std::hypot(point.x, point.y);
+      max_dist = max_dist < dist ? dist : max_dist;
+    }
+    return max_dist;
+  } else {
+    return std::nullopt;
+  }
+}
+
+Validator3D::Validator3D(PointsNumThresholdParam & points_num_threshold_param)
+: Validator(points_num_threshold_param)
+{
+}
+bool Validator3D::setKdtreeInputCloud(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_cloud)
+{
+  obstacle_pointcloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::fromROSMsg(*input_cloud, *obstacle_pointcloud_);
+  if (obstacle_pointcloud_->empty()) {
+    return false;
+  }
+  // setup kdtree_
+  kdtree_ = pcl::make_shared<pcl::search::KdTree<pcl::PointXYZ>>(false);
+  kdtree_->setInputCloud(obstacle_pointcloud_);
+  return true;
+}
+std::optional<float> Validator3D::getMaxRadius(
+  const autoware_auto_perception_msgs::msg::DetectedObject & object)
+{
+  if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) {
+    auto square_radius = (object.shape.dimensions.x * 0.5f) * (object.shape.dimensions.x * 0.5f) +
+                         (object.shape.dimensions.y * 0.5f) * (object.shape.dimensions.y * 0.5f) +
+                         (object.shape.dimensions.z * 0.5f) * (object.shape.dimensions.z * 0.5f);
+    return std::sqrt(square_radius);
+  } else if (object.shape.type == Shape::POLYGON) {
+    float max_dist = 0.0;
+    for (const auto & point : object.shape.footprint.points) {
+      const float dist = std::hypot(point.x, point.y);
+      max_dist = max_dist < dist ? dist : max_dist;
+    }
+    return std::hypot(max_dist, object.shape.dimensions.z * 0.5f);
+  } else {
+    return std::nullopt;
+  }
+}
+
+std::optional<size_t> Validator3D::getPointCloudWithinObject(
+  const autoware_auto_perception_msgs::msg::DetectedObject & object,
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr neighbor_pointcloud)
+{
+  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.x;
+  auto z_min = object_position.z - object_height / 2.0f;
+  auto 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(), 0.0);
+  }
+
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_pointcloud_2d(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::CropHull<pcl::PointXYZ> cropper;
+  cropper.setInputCloud(neighbor_pointcloud);
+  cropper.setDim(2);
+  cropper.setHullIndices(vertices_array);
+  cropper.setHullCloud(poly3d);
+  cropper.setCropOutside(true);
+  cropper.filter(*cropped_pointcloud_2d);
+
+  cropped_pointcloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
+  cropped_pointcloud_->reserve(cropped_pointcloud_2d->size());
+  for (const auto & point : *cropped_pointcloud_2d) {
+    if (point.z > z_min && point.z < z_max) {
+      cropped_pointcloud_->push_back(point);
+    }
+  }
+  return cropped_pointcloud_->size();
+}
+
+bool Validator3D::validate_object(
+  const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object)
+{
+  neighbor_pointcloud_.reset(new pcl::PointCloud<pcl::PointXYZ>);
+  std::vector<int> indices;
+  std::vector<float> distances;
+  const auto search_radius = getMaxRadius(transformed_object);
+  if (!search_radius) {
+    return false;
+  }
+  kdtree_->radiusSearch(
+    pcl::PointXYZ(
+      transformed_object.kinematics.pose_with_covariance.pose.position.x,
+      transformed_object.kinematics.pose_with_covariance.pose.position.y,
+      transformed_object.kinematics.pose_with_covariance.pose.position.z),
+    search_radius.value(), indices, distances);
+  for (const auto & index : indices) {
+    neighbor_pointcloud_->push_back(obstacle_pointcloud_->at(index));
+  }
+
+  const auto num = getPointCloudWithinObject(transformed_object, neighbor_pointcloud_);
+  if (!num) return true;
+
+  size_t threshold_pointcloud_num = getThresholdPointCloud(transformed_object);
+  if (num.value() > threshold_pointcloud_num) {
+    return true;
+  }
+  return false;  // remove object
+}
 
 ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
   const rclcpp::NodeOptions & node_options)
@@ -85,24 +282,32 @@ 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;
+
+  sync_.registerCallback(
+    std::bind(&ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud, this, _1, _2));
+  if (using_2d_validator_) {
+    validator_ = std::make_unique<Validator2D>(points_num_threshold_param_);
+  } else {
+    validator_ = std::make_unique<Validator3D>(points_num_threshold_param_);
+  }
+
+  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::onObjectsAndObstaclePointCloud(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr & input_objects,
   const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_obstacle_pointcloud)
@@ -119,61 +324,23 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
     // objects_pub_->publish(*input_objects);
     return;
   }
-
-  // Convert to PCL
-  pcl::PointCloud<pcl::PointXY>::Ptr obstacle_pointcloud(new pcl::PointCloud<pcl::PointXY>);
-  pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud);
-  if (obstacle_pointcloud->empty()) {
+  if (!validator_->setKdtreeInputCloud(input_obstacle_pointcloud)) {
     RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud");
-    // objects_pub_->publish(*input_objects);
     return;
   }
 
-  // Create Kd-tree to search neighbor pointcloud to reduce cost.
-  pcl::search::Search<pcl::PointXY>::Ptr kdtree =
-    pcl::make_shared<pcl::search::KdTree<pcl::PointXY>>(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::PointXY>::Ptr neighbor_pointcloud(new pcl::PointCloud<pcl::PointXY>);
-    std::vector<int> indices;
-    std::vector<float> distances;
-    kdtree->radiusSearch(
-      toPCL(transformed_object_position), search_radius.value(), indices, distances);
-    for (const auto & index : indices) {
-      neighbor_pointcloud->push_back(obstacle_pointcloud->at(index));
+    const auto validated = validator_->validate_object(transformed_object);
+    if (debugger_) {
+      debugger_->addNeighborPointcloud(validator_->getDebugNeighborPointCloud());
+      debugger_->addPointcloudWithinPolygon(validator_->getDebugPointCloudWithinObject());
     }
-    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.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 {
+    if (validated) {
       output.objects.push_back(object);
+    } else {
+      removed_objects.objects.push_back(object);
     }
   }
 
@@ -185,56 +352,6 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
   }
 }
 
-std::optional<size_t> ObstaclePointCloudBasedValidator::getPointCloudNumWithinPolygon(
-  const autoware_auto_perception_msgs::msg::DetectedObject & object,
-  const pcl::PointCloud<pcl::PointXY>::Ptr pointcloud)
-{
-  pcl::PointCloud<pcl::PointXYZ>::Ptr cropped_pointcloud(new pcl::PointCloud<pcl::PointXYZ>);
-  std::vector<pcl::Vertices> vertices_array;
-  pcl::Vertices vertices;
-
-  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(), 0.0);
-  }
-
-  pcl::CropHull<pcl::PointXYZ> cropper;  // don't be implemented PointXY by PCL
-  cropper.setInputCloud(toXYZ(pointcloud));
-  cropper.setDim(2);
-  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<float> ObstaclePointCloudBasedValidator::getMaxRadius(
-  const autoware_auto_perception_msgs::msg::DetectedObject & object)
-{
-  if (object.shape.type == Shape::BOUNDING_BOX || object.shape.type == Shape::CYLINDER) {
-    return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
-  } else if (object.shape.type == Shape::POLYGON) {
-    float max_dist = 0.0;
-    for (const auto & point : object.shape.footprint.points) {
-      const float dist = std::hypot(point.x, point.y);
-      max_dist = max_dist < dist ? dist : max_dist;
-    }
-    return max_dist;
-  } else {
-    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5000, "unknown shape type");
-    return std::nullopt;
-  }
-}
-
 }  // namespace obstacle_pointcloud_based_validator
 
 #include <rclcpp_components/register_node_macro.hpp>