Revert "refactor(detected_object_validation): apply util functions de…

…fined in the other packages (#2093)"

This reverts commit 206bcdb.

Signed-off-by: ktro2828 <kotaro.uetake@tier4.jp>

perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp

@@ -15,8 +15,6 @@
 #ifndef DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_
 #define DETECTED_OBJECT_FILTER__OBJECT_LANELET_FILTER_HPP_
 
-#include "utils/utils.hpp"
-
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -37,6 +35,18 @@ using tier4_autoware_utils::MultiPoint2d;
 using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
 
+struct Filter_target_label
+{
+  bool UNKNOWN;
+  bool CAR;
+  bool TRUCK;
+  bool BUS;
+  bool TRAILER;
+  bool MOTORCYCLE;
+  bool BICYCLE;
+  bool PEDESTRIAN;
+};
+
 class ObjectLaneletFilterNode : public rclcpp::Node
 {
 public:
@@ -57,7 +67,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
-  utils::FilterTargetLabel filter_target_;
+  Filter_target_label filter_target_;
 
   LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
   lanelet::ConstLanelets getIntersectedLanelets(

perception/detected_object_validation/include/detected_object_filter/object_position_filter.hpp

@@ -15,8 +15,6 @@
 #ifndef DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_
 #define DETECTED_OBJECT_FILTER__OBJECT_POSITION_FILTER_HPP_
 
-#include "utils/utils.hpp"
-
 #include <lanelet2_extension/utility/message_conversion.hpp>
 #include <lanelet2_extension/utility/query.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -32,6 +30,17 @@
 
 namespace object_position_filter
 {
+struct Filter_target_label
+{
+  bool UNKNOWN;
+  bool CAR;
+  bool TRUCK;
+  bool BUS;
+  bool TRAILER;
+  bool MOTORCYCLE;
+  bool BICYCLE;
+  bool PEDESTRIAN;
+};
 
 class ObjectPositionFilterNode : public rclcpp::Node
 {
@@ -51,8 +60,7 @@ class ObjectPositionFilterNode : public rclcpp::Node
   float upper_bound_y_;
   float lower_bound_x_;
   float lower_bound_y_;
-  utils::FilterTargetLabel filter_target_;
-  bool isObjectInBounds(const autoware_auto_perception_msgs::msg::DetectedObject & object) const;
+  Filter_target_label filter_target_;
 };
 
 }  // namespace object_position_filter

perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp

@@ -63,6 +63,9 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
   std::optional<size_t> getPointCloudNumWithinPolygon(
     const autoware_auto_perception_msgs::msg::DetectedObject & object,
     const pcl::PointCloud<pcl::PointXY>::Ptr pointcloud);
+  void toPolygon2d(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const pcl::PointCloud<pcl::PointXY>::Ptr & polygon);
   std::optional<float> getMaxRadius(
     const autoware_auto_perception_msgs::msg::DetectedObject & object);
 };

perception/detected_object_validation/include/occupancy_grid_based_validator/occupancy_grid_based_validator.hpp

@@ -62,6 +62,9 @@ class OccupancyGridBasedValidator : public rclcpp::Node
   std::optional<cv::Mat> getMask(
     const nav_msgs::msg::OccupancyGrid & occupancy_grid,
     const autoware_auto_perception_msgs::msg::DetectedObject & object, cv::Mat mask);
+  void toPolygon2d(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    std::vector<cv::Point2f> & vertices);
   void showDebugImage(
     const nav_msgs::msg::OccupancyGrid & ros_occ_grid,
     const autoware_auto_perception_msgs::msg::DetectedObjects & objects, const cv::Mat & occ_grid);

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -65,6 +65,8 @@ void ObjectLaneletFilterNode::mapCallback(
 void ObjectLaneletFilterNode::objectCallback(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_msg)
 {
+  using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
+
   // Guard
   if (object_pub_->get_subscription_count() < 1) return;
 
@@ -91,7 +93,15 @@ void ObjectLaneletFilterNode::objectCallback(
   for (const auto & object : transformed_objects.objects) {
     const auto & footprint = object.shape.footprint;
     const auto & label = object.classification.front().label;
-    if (filter_target_.isTarget(label)) {
+    if (
+      (label == Label::UNKNOWN && filter_target_.UNKNOWN) ||
+      (label == Label::CAR && filter_target_.CAR) ||
+      (label == Label::TRUCK && filter_target_.TRUCK) ||
+      (label == Label::BUS && filter_target_.BUS) ||
+      (label == Label::TRAILER && filter_target_.TRAILER) ||
+      (label == Label::MOTORCYCLE && filter_target_.MOTORCYCLE) ||
+      (label == Label::BICYCLE && filter_target_.BICYCLE) ||
+      (label == Label::PEDESTRIAN && filter_target_.PEDESTRIAN)) {
       Polygon2d polygon;
       for (const auto & point : footprint.points) {
         const geometry_msgs::msg::Point32 point_transformed =

perception/detected_object_validation/src/object_position_filter.cpp

@@ -49,16 +49,28 @@ ObjectPositionFilterNode::ObjectPositionFilterNode(const rclcpp::NodeOptions & n
 void ObjectPositionFilterNode::objectCallback(
   const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr input_msg)
 {
+  using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
   // Guard
   if (object_pub_->get_subscription_count() < 1) return;
 
   autoware_auto_perception_msgs::msg::DetectedObjects output_object_msg;
   output_object_msg.header = input_msg->header;
 
   for (const auto & object : input_msg->objects) {
+    const auto & position = object.kinematics.pose_with_covariance.pose.position;
     const auto & label = object.classification.front().label;
-    if (filter_target_.isTarget(label)) {
-      if (isObjectInBounds(object)) {
+    if (
+      (label == Label::UNKNOWN && filter_target_.UNKNOWN) ||
+      (label == Label::CAR && filter_target_.CAR) ||
+      (label == Label::TRUCK && filter_target_.TRUCK) ||
+      (label == Label::BUS && filter_target_.BUS) ||
+      (label == Label::TRAILER && filter_target_.TRAILER) ||
+      (label == Label::MOTORCYCLE && filter_target_.MOTORCYCLE) ||
+      (label == Label::BICYCLE && filter_target_.BICYCLE) ||
+      (label == Label::PEDESTRIAN && filter_target_.PEDESTRIAN)) {
+      if (
+        position.x > lower_bound_x_ && position.x < upper_bound_x_ && position.y > lower_bound_y_ &&
+        position.y < upper_bound_y_) {
         output_object_msg.objects.emplace_back(object);
       }
     } else {
@@ -69,14 +81,6 @@ void ObjectPositionFilterNode::objectCallback(
   object_pub_->publish(output_object_msg);
 }
 
-bool ObjectPositionFilterNode::isObjectInBounds(
-  const autoware_auto_perception_msgs::msg::DetectedObject & object) const
-{
-  const auto & position = object.kinematics.pose_with_covariance.pose.position;
-  return position.x > lower_bound_x_ && position.x < upper_bound_x_ &&
-         position.y > lower_bound_y_ && position.y < upper_bound_y_;
-}
-
 }  // namespace object_position_filter
 
 #include <rclcpp_components/register_node_macro.hpp>

perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp

@@ -17,8 +17,6 @@
 #include <perception_utils/perception_utils.hpp>
 #include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
-#include <boost/geometry.hpp>
-
 #include <pcl/filters/crop_hull.h>
 #include <pcl/kdtree/kdtree_flann.h>
 #include <pcl/search/kdtree.h>
@@ -70,9 +68,8 @@ inline pcl::PointCloud<pcl::PointXYZ>::Ptr toXYZ(
 
 namespace obstacle_pointcloud_based_validator
 {
-namespace bg = boost::geometry;
+using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
 using Shape = autoware_auto_perception_msgs::msg::Shape;
-using Polygon2d = tier4_autoware_utils::Polygon2d;
 
 ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
   const rclcpp::NodeOptions & node_options)
@@ -119,7 +116,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
   pcl::PointCloud<pcl::PointXY>::Ptr obstacle_pointcloud(new pcl::PointCloud<pcl::PointXY>);
   pcl::fromROSMsg(*input_obstacle_pointcloud, *obstacle_pointcloud);
   if (obstacle_pointcloud->empty()) {
-    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud");
+    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receieve pointcloud");
     // objects_pub_->publish(*input_objects);
     return;
   }
@@ -153,8 +150,10 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
     // Filter object that have few pointcloud in them.
     const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud);
     if (num) {
-      (min_pointcloud_num_ <= num.value()) ? output.objects.push_back(object)
-                                           : removed_objects.objects.push_back(object);
+      if (min_pointcloud_num_ <= num.value())
+        output.objects.push_back(object);
+      else
+        removed_objects.objects.push_back(object);
     } else {
       output.objects.push_back(object);
     }
@@ -172,38 +171,85 @@ std::optional<size_t> ObstaclePointCloudBasedValidator::getPointCloudNumWithinPo
   const autoware_auto_perception_msgs::msg::DetectedObject & object,
   const pcl::PointCloud<pcl::PointXY>::Ptr pointcloud)
 {
+  pcl::PointCloud<pcl::PointXY>::Ptr polygon(new pcl::PointCloud<pcl::PointXY>);
   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>);
+  toPolygon2d(object, polygon);
+  if (polygon->empty()) return std::nullopt;
 
-  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);
-  }
+  for (size_t i = 0; i < polygon->size(); ++i) vertices.vertices.push_back(i);
+  vertices_array.push_back(vertices);
 
   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.setHullCloud(toXYZ(polygon));
   cropper.setCropOutside(true);
   cropper.filter(*cropped_pointcloud);
 
   if (debugger_) debugger_->addPointcloudWithinPolygon(cropped_pointcloud);
   return cropped_pointcloud->size();
 }
 
+void ObstaclePointCloudBasedValidator::toPolygon2d(
+  const autoware_auto_perception_msgs::msg::DetectedObject & object,
+  const pcl::PointCloud<pcl::PointXY>::Ptr & polygon)
+{
+  if (object.shape.type == Shape::BOUNDING_BOX) {
+    const auto & pose = object.kinematics.pose_with_covariance.pose;
+    double yaw = tf2::getYaw(pose.orientation);
+    Eigen::Matrix2d rotation;
+    rotation << std::cos(yaw), -std::sin(yaw), std::sin(yaw), std::cos(yaw);
+    Eigen::Vector2d offset0, offset1, offset2, offset3;
+    offset0 = rotation *
+              Eigen::Vector2d(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
+    offset1 = rotation *
+              Eigen::Vector2d(object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
+    offset2 = rotation *
+              Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, -object.shape.dimensions.y * 0.5f);
+    offset3 = rotation *
+              Eigen::Vector2d(-object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
+    polygon->push_back(
+      pcl::PointXY(toPCL(pose.position.x + offset0.x(), pose.position.y + offset0.y())));
+    polygon->push_back(
+      pcl::PointXY(toPCL(pose.position.x + offset1.x(), pose.position.y + offset1.y())));
+    polygon->push_back(
+      pcl::PointXY(toPCL(pose.position.x + offset2.x(), pose.position.y + offset2.y())));
+    polygon->push_back(
+      pcl::PointXY(toPCL(pose.position.x + offset3.x(), pose.position.y + offset3.y())));
+  } else if (object.shape.type == Shape::CYLINDER) {
+    const auto & center = object.kinematics.pose_with_covariance.pose.position;
+    const auto & radius = object.shape.dimensions.x * 0.5;
+    constexpr int n = 6;
+    for (int i = 0; i < n; ++i) {
+      Eigen::Vector2d point;
+      point.x() = std::cos(
+                    (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
+                    M_PI / static_cast<double>(n)) *
+                    radius +
+                  center.x;
+      point.y() = std::sin(
+                    (static_cast<double>(i) / static_cast<double>(n)) * 2.0 * M_PI +
+                    M_PI / static_cast<double>(n)) *
+                    radius +
+                  center.y;
+      polygon->push_back(toPCL(point.x(), point.y()));
+    }
+  } else if (object.shape.type == Shape::POLYGON) {
+    RCLCPP_WARN_THROTTLE(
+      this->get_logger(), *this->get_clock(), 5000, "POLYGON type is not supported");
+  }
+}
+
 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) {
+  if (object.shape.type == Shape::BOUNDING_BOX) {
+    return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
+  } else if (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;