feat(detected_object_validation): filter unknown objects with its foo…

…tprint (#7186) feat: filter unknown objects with its footprint Signed-off-by: yoshiri <[email protected]>
autowarefoundation · Jun 3, 2024 · cb739f1 · cb739f1
1 parent e132ad7
commit cb739f1
Show file tree

Hide file tree

Showing 3 changed files with 55 additions and 11 deletions.
diff --git a/...ation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/...ation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp
@@ -80,6 +80,9 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
   lanelet::ConstLanelets getIntersectedLanelets(
     const LinearRing2d &, const lanelet::ConstLanelets &);
+  bool isObjectOverlapLanelets(
+    const autoware_auto_perception_msgs::msg::DetectedObject & object,
+    const lanelet::ConstLanelets & intersected_lanelets);
   bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &);
   bool isSameDirectionWithLanelets(
     const lanelet::ConstLanelets & lanelets,

diff --git a/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp b/perception/detected_object_validation/include/detected_object_validation/utils/utils.hpp
@@ -15,8 +15,9 @@
 #ifndef DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_
 #define DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_
 
-#include <cstdint>
+#include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
 
+#include <cstdint>
 namespace utils
 {
 struct FilterTargetLabel
@@ -31,6 +32,21 @@ struct FilterTargetLabel
   bool PEDESTRIAN;
   bool isTarget(const uint8_t label) const;
 };  // struct FilterTargetLabel
+
+inline bool hasBoundingBox(const autoware_auto_perception_msgs::msg::DetectedObject & object)
+{
+  if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
+    return true;
+  } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
+    return true;
+  } else if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) {
+    return false;
+  } else {
+    // unknown shape type.
+    return false;
+  }
+}
+
 }  // namespace utils
 
 #endif  // DETECTED_OBJECT_VALIDATION__UTILS__UTILS_HPP_
diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp
@@ -139,17 +139,9 @@ bool ObjectLaneletFilterNode::filterObject(
     bool filter_pass = true;
     // 1. is polygon overlap with road lanelets or shoulder lanelets
     if (filter_settings_.polygon_overlap_filter) {
-      Polygon2d polygon;
-      const auto footprint = setFootprint(transformed_object);
-      for (const auto & point : footprint.points) {
-        const geometry_msgs::msg::Point32 point_transformed = tier4_autoware_utils::transformPoint(
-          point, transformed_object.kinematics.pose_with_covariance.pose);
-        polygon.outer().emplace_back(point_transformed.x, point_transformed.y);
-      }
-      polygon.outer().push_back(polygon.outer().front());
       const bool is_polygon_overlap =
-        isPolygonOverlapLanelets(polygon, intersected_road_lanelets) ||
-        isPolygonOverlapLanelets(polygon, intersected_shoulder_lanelets);
+        isObjectOverlapLanelets(transformed_object, intersected_road_lanelets) ||
+        isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets);
       filter_pass = filter_pass && is_polygon_overlap;
     }
 
@@ -234,6 +226,39 @@ lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets(
   return intersected_lanelets;
 }
 
+bool ObjectLaneletFilterNode::isObjectOverlapLanelets(
+  const autoware_auto_perception_msgs::msg::DetectedObject & object,
+  const lanelet::ConstLanelets & intersected_lanelets)
+{
+  // if has bounding box, use polygon overlap
+  if (utils::hasBoundingBox(object)) {
+    Polygon2d polygon;
+    const auto footprint = setFootprint(object);
+    for (const auto & point : footprint.points) {
+      const geometry_msgs::msg::Point32 point_transformed =
+        tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose);
+      polygon.outer().emplace_back(point_transformed.x, point_transformed.y);
+    }
+    polygon.outer().push_back(polygon.outer().front());
+    return isPolygonOverlapLanelets(polygon, intersected_lanelets);
+  } else {
+    // if object do not have bounding box, check each footprint is inside polygon
+    for (const auto & point : object.shape.footprint.points) {
+      const geometry_msgs::msg::Point32 point_transformed =
+        tier4_autoware_utils::transformPoint(point, object.kinematics.pose_with_covariance.pose);
+      geometry_msgs::msg::Pose point2d;
+      point2d.position.x = point_transformed.x;
+      point2d.position.y = point_transformed.y;
+      for (const auto & lanelet : intersected_lanelets) {
+        if (lanelet::utils::isInLanelet(point2d, lanelet, 0.0)) {
+          return true;
+        }
+      }
+    }
+    return false;
+  }
+}
+
 bool ObjectLaneletFilterNode::isPolygonOverlapLanelets(
   const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets)
 {