diff --git a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp index 00826de68157f..6c652d0ad093e 100644 --- a/perception/detected_object_validation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp +++ b/perception/detected_object_validation/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 index 2c46e1b9dd110..853733dc0ee5f 100644 --- 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 +#include +#include 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 index fe304a3ea22bb..80a2ce4b6563f 100644 --- 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) {