refactor(detected_object_validation): apply util functions defined in…

… the other packages (#2093)

* refactor: add FilterTargetLabel to utils

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

* refactor: apply isCarLikeVehicle()

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

* refactor: apply tier4_autoware_tuils::toPolygon2d()

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

* refactor: update misc

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

* refactor: replace useless variable with function

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

* refactor: update to loop elements of polygon with index

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

* refactor: update to divide validation function into a method

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

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

perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp

@@ -15,6 +15,8 @@
 #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>
@@ -35,18 +37,6 @@ 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:
@@ -67,7 +57,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
-  Filter_target_label filter_target_;
+  utils::FilterTargetLabel 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,6 +15,8 @@
 #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>
@@ -30,17 +32,6 @@
 
 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
 {
@@ -60,7 +51,8 @@ class ObjectPositionFilterNode : public rclcpp::Node
   float upper_bound_y_;
   float lower_bound_x_;
   float lower_bound_y_;
-  Filter_target_label filter_target_;
+  utils::FilterTargetLabel filter_target_;
+  bool isObjectInBounds(const autoware_auto_perception_msgs::msg::DetectedObject & object) const;
 };
 
 }  // namespace object_position_filter

perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp

@@ -63,9 +63,6 @@ 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,9 +62,6 @@ 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/include/utils/utils.hpp

@@ -0,0 +1,36 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef UTILS__UTILS_HPP_
+#define UTILS__UTILS_HPP_
+
+#include <cstdint>
+
+namespace utils
+{
+struct FilterTargetLabel
+{
+  bool UNKNOWN;
+  bool CAR;
+  bool TRUCK;
+  bool BUS;
+  bool TRAILER;
+  bool MOTORCYCLE;
+  bool BICYCLE;
+  bool PEDESTRIAN;
+  bool isTarget(const uint8_t label) const;
+};  // struct FilterTargetLabel
+}  // namespace utils
+
+#endif  // UTILS__UTILS_HPP_

perception/detected_object_validation/src/object_lanelet_filter.cpp

@@ -65,8 +65,6 @@ 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;
 
@@ -93,15 +91,7 @@ void ObjectLaneletFilterNode::objectCallback(
   for (const auto & object : transformed_objects.objects) {
     const auto & footprint = object.shape.footprint;
     const auto & label = object.classification.front().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)) {
+    if (filter_target_.isTarget(label)) {
       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,28 +49,16 @@ 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 (
-      (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_) {
+    if (filter_target_.isTarget(label)) {
+      if (isObjectInBounds(object)) {
         output_object_msg.objects.emplace_back(object);
       }
     } else {
@@ -81,6 +69,14 @@ 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,6 +17,8 @@
 #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>
@@ -68,8 +70,9 @@ inline pcl::PointCloud<pcl::PointXYZ>::Ptr toXYZ(
 
 namespace obstacle_pointcloud_based_validator
 {
-using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
+namespace bg = boost::geometry;
 using Shape = autoware_auto_perception_msgs::msg::Shape;
+using Polygon2d = tier4_autoware_utils::Polygon2d;
 
 ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
   const rclcpp::NodeOptions & node_options)
@@ -116,7 +119,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 receieve pointcloud");
+    RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 5, "cannot receive pointcloud");
     // objects_pub_->publish(*input_objects);
     return;
   }
@@ -150,10 +153,8 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
     // Filter object that have few pointcloud in them.
     const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud);
     if (num) {
-      if (min_pointcloud_num_ <= num.value())
-        output.objects.push_back(object);
-      else
-        removed_objects.objects.push_back(object);
+      (min_pointcloud_num_ <= num.value()) ? output.objects.push_back(object)
+                                           : removed_objects.objects.push_back(object);
     } else {
       output.objects.push_back(object);
     }
@@ -171,85 +172,38 @@ 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;
 
-  toPolygon2d(object, polygon);
-  if (polygon->empty()) return std::nullopt;
+  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 < polygon->size(); ++i) vertices.vertices.push_back(i);
-  vertices_array.push_back(vertices);
+  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(toXYZ(polygon));
+  cropper.setHullCloud(poly3d);
   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) {
-    return std::hypot(object.shape.dimensions.x * 0.5f, object.shape.dimensions.y * 0.5f);
-  } else if (object.shape.type == Shape::CYLINDER) {
+  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;