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 2598f6ec6ca74..850de6d4a6f89 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 @@ -26,12 +26,17 @@ #include #include +#include + #include +#include #include #include #include #include +#include +#include namespace object_lanelet_filter { @@ -40,6 +45,13 @@ using autoware::universe_utils::MultiPoint2d; using autoware::universe_utils::Point2d; using autoware::universe_utils::Polygon2d; +namespace bg = boost::geometry; +namespace bgi = boost::geometry::index; +using Point2d = bg::model::point; +using Box = boost::geometry::model::box; +using BoxAndLanelet = std::pair; +using RtreeAlgo = bgi::rstar<16>; + class ObjectLaneletFilterNode : public rclcpp::Node { public: @@ -72,17 +84,20 @@ class ObjectLaneletFilterNode : public rclcpp::Node bool filterObject( const autoware_perception_msgs::msg::DetectedObject & transformed_object, const autoware_perception_msgs::msg::DetectedObject & input_object, - const lanelet::ConstLanelets & intersected_lanelets, + const bg::index::rtree & local_rtree, autoware_perception_msgs::msg::DetectedObjects & output_object_msg); LinearRing2d getConvexHull(const autoware_perception_msgs::msg::DetectedObjects &); - lanelet::ConstLanelets getIntersectedLanelets(const LinearRing2d &); + LinearRing2d getConvexHullFromObjectFootprint( + const autoware_perception_msgs::msg::DetectedObject & object); + std::vector getIntersectedLanelets(const LinearRing2d &); bool isObjectOverlapLanelets( const autoware_perception_msgs::msg::DetectedObject & object, - const lanelet::ConstLanelets & intersected_lanelets); - bool isPolygonOverlapLanelets(const Polygon2d &, const lanelet::ConstLanelets &); + const bg::index::rtree & local_rtree); + bool isPolygonOverlapLanelets( + const Polygon2d & polygon, const bgi::rtree & local_rtree); bool isSameDirectionWithLanelets( - const lanelet::ConstLanelets & lanelets, - const autoware_perception_msgs::msg::DetectedObject & object); + const autoware_perception_msgs::msg::DetectedObject & object, + const bgi::rtree & local_rtree); geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &); std::unique_ptr published_time_publisher_; diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp index 506fefdda1dbb..a439c744f9288 100644 --- a/perception/detected_object_validation/src/object_lanelet_filter.cpp +++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp @@ -22,8 +22,10 @@ #include #include #include +#include #include +#include #include #include @@ -98,18 +100,27 @@ void ObjectLaneletFilterNode::objectCallback( return; } - // calculate convex hull - const auto convex_hull = getConvexHull(transformed_objects); + if (!transformed_objects.objects.empty()) { + // calculate convex hull + const auto convex_hull = getConvexHull(transformed_objects); - // get intersected lanelets - lanelet::ConstLanelets intersected_lanelets = getIntersectedLanelets(convex_hull); + // get intersected lanelets + std::vector intersected_lanelets_with_bbox = getIntersectedLanelets(convex_hull); - // filtering process - for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { - const auto & transformed_object = transformed_objects.objects.at(index); - const auto & input_object = input_msg->objects.at(index); - filterObject(transformed_object, input_object, intersected_lanelets, output_object_msg); + // create R-Tree with intersected_lanelets for fast search + bgi::rtree local_rtree; + for (const auto & bbox_and_lanelet : intersected_lanelets_with_bbox) { + local_rtree.insert(bbox_and_lanelet); + } + + // filtering process + for (size_t index = 0; index < transformed_objects.objects.size(); ++index) { + const auto & transformed_object = transformed_objects.objects.at(index); + const auto & input_object = input_msg->objects.at(index); + filterObject(transformed_object, input_object, local_rtree, output_object_msg); + } } + object_pub_->publish(output_object_msg); published_time_publisher_->publish_if_subscribed(object_pub_, output_object_msg.header.stamp); @@ -126,16 +137,20 @@ void ObjectLaneletFilterNode::objectCallback( bool ObjectLaneletFilterNode::filterObject( const autoware_perception_msgs::msg::DetectedObject & transformed_object, const autoware_perception_msgs::msg::DetectedObject & input_object, - const lanelet::ConstLanelets & intersected_lanelets, + const bgi::rtree & local_rtree, autoware_perception_msgs::msg::DetectedObjects & output_object_msg) { const auto & label = transformed_object.classification.front().label; if (filter_target_.isTarget(label)) { + // no tree, then no intersection + if (local_rtree.empty()) { + return false; + } + bool filter_pass = true; // 1. is polygon overlap with road lanelets or shoulder lanelets if (filter_settings_.polygon_overlap_filter) { - const bool is_polygon_overlap = - isObjectOverlapLanelets(transformed_object, intersected_lanelets); + const bool is_polygon_overlap = isObjectOverlapLanelets(transformed_object, local_rtree); filter_pass = filter_pass && is_polygon_overlap; } @@ -144,8 +159,7 @@ bool ObjectLaneletFilterNode::filterObject( transformed_object.kinematics.orientation_availability == autoware_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE; if (filter_settings_.lanelet_direction_filter && !orientation_not_available) { - const bool is_same_direction = - isSameDirectionWithLanelets(intersected_lanelets, transformed_object); + const bool is_same_direction = isSameDirectionWithLanelets(transformed_object, local_rtree); filter_pass = filter_pass && is_same_direction; } @@ -197,55 +211,75 @@ LinearRing2d ObjectLaneletFilterNode::getConvexHull( candidate_points.emplace_back(p.x + pos.x, p.y + pos.y); } } + LinearRing2d convex_hull; + bg::convex_hull(candidate_points, convex_hull); + + return convex_hull; +} + +LinearRing2d ObjectLaneletFilterNode::getConvexHullFromObjectFootprint( + const autoware_perception_msgs::msg::DetectedObject & object) +{ + MultiPoint2d candidate_points; + const auto & pos = object.kinematics.pose_with_covariance.pose.position; + const auto footprint = setFootprint(object); + + for (const auto & p : footprint.points) { + candidate_points.emplace_back(p.x + pos.x, p.y + pos.y); + } LinearRing2d convex_hull; - boost::geometry::convex_hull(candidate_points, convex_hull); + bg::convex_hull(candidate_points, convex_hull); return convex_hull; } // fetch the intersected candidate lanelets with bounding box and then // check the intersections among the lanelets and the convex hull -lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets( +std::vector ObjectLaneletFilterNode::getIntersectedLanelets( const LinearRing2d & convex_hull) { - namespace bg = boost::geometry; - - lanelet::ConstLanelets intersected_lanelets; + std::vector intersected_lanelets_with_bbox; // convert convex_hull to a 2D bounding box for searching in the LaneletMap - bg::model::box> bbox2d_convex_hull; - bg::envelope(convex_hull, bbox2d_convex_hull); - lanelet::BoundingBox2d bbox2d( + bg::model::box> bbox_of_convex_hull; + bg::envelope(convex_hull, bbox_of_convex_hull); + const lanelet::BoundingBox2d bbox2d( lanelet::BasicPoint2d( - bg::get(bbox2d_convex_hull), - bg::get(bbox2d_convex_hull)), + bg::get(bbox_of_convex_hull), + bg::get(bbox_of_convex_hull)), lanelet::BasicPoint2d( - bg::get(bbox2d_convex_hull), - bg::get(bbox2d_convex_hull))); + bg::get(bbox_of_convex_hull), + bg::get(bbox_of_convex_hull))); - lanelet::Lanelets candidates_lanelets = lanelet_map_ptr_->laneletLayer.search(bbox2d); - for (const auto & lanelet : candidates_lanelets) { + const lanelet::Lanelets candidate_lanelets = lanelet_map_ptr_->laneletLayer.search(bbox2d); + for (const auto & lanelet : candidate_lanelets) { // only check the road lanelets and road shoulder lanelets if ( lanelet.hasAttribute(lanelet::AttributeName::Subtype) && (lanelet.attribute(lanelet::AttributeName::Subtype).value() == lanelet::AttributeValueString::Road || lanelet.attribute(lanelet::AttributeName::Subtype).value() == "road_shoulder")) { - if (boost::geometry::intersects(convex_hull, lanelet.polygon2d().basicPolygon())) { - intersected_lanelets.emplace_back(lanelet); + if (bg::intersects(convex_hull, lanelet.polygon2d().basicPolygon())) { + // create bbox using boost for making the R-tree in later phase + lanelet::BoundingBox2d lanelet_bbox = lanelet::geometry::boundingBox2d(lanelet); + Point2d min_corner(lanelet_bbox.min().x(), lanelet_bbox.min().y()); + Point2d max_corner(lanelet_bbox.max().x(), lanelet_bbox.max().y()); + Box boost_bbox(min_corner, max_corner); + + intersected_lanelets_with_bbox.emplace_back(std::make_pair(boost_bbox, lanelet)); } } } - return intersected_lanelets; + return intersected_lanelets_with_bbox; } bool ObjectLaneletFilterNode::isObjectOverlapLanelets( const autoware_perception_msgs::msg::DetectedObject & object, - const lanelet::ConstLanelets & intersected_lanelets) + const bgi::rtree & local_rtree) { - // if has bounding box, use polygon overlap + // if object has bounding box, use polygon overlap if (utils::hasBoundingBox(object)) { Polygon2d polygon; const auto footprint = setFootprint(object); @@ -256,8 +290,17 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( polygon.outer().emplace_back(point_transformed.x, point_transformed.y); } polygon.outer().push_back(polygon.outer().front()); - return isPolygonOverlapLanelets(polygon, intersected_lanelets); + + return isPolygonOverlapLanelets(polygon, local_rtree); } else { + const LinearRing2d object_convex_hull = getConvexHullFromObjectFootprint(object); + + // create bounding box to search in the rtree + std::vector candidates; + bg::model::box> bbox; + bg::envelope(object_convex_hull, bbox); + local_rtree.query(bgi::intersects(bbox), std::back_inserter(candidates)); + // 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 = @@ -266,30 +309,39 @@ bool ObjectLaneletFilterNode::isObjectOverlapLanelets( 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)) { + + for (const auto & candidate : candidates) { + if (lanelet::utils::isInLanelet(point2d, candidate.second, 0.0)) { return true; } } } + return false; } } bool ObjectLaneletFilterNode::isPolygonOverlapLanelets( - const Polygon2d & polygon, const lanelet::ConstLanelets & intersected_lanelets) + const Polygon2d & polygon, const bgi::rtree & local_rtree) { - for (const auto & lanelet : intersected_lanelets) { - if (!boost::geometry::disjoint(polygon, lanelet.polygon2d().basicPolygon())) { + // create a bounding box from polygon for searching the local R-tree + std::vector candidates; + bg::model::box> bbox_of_convex_hull; + bg::envelope(polygon, bbox_of_convex_hull); + local_rtree.query(bgi::intersects(bbox_of_convex_hull), std::back_inserter(candidates)); + + for (const auto & box_and_lanelet : candidates) { + if (!bg::disjoint(polygon, box_and_lanelet.second.polygon2d().basicPolygon())) { return true; } } + return false; } bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( - const lanelet::ConstLanelets & lanelets, - const autoware_perception_msgs::msg::DetectedObject & object) + const autoware_perception_msgs::msg::DetectedObject & object, + const bgi::rtree & local_rtree) { const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation); const double object_velocity_norm = std::hypot( @@ -303,14 +355,30 @@ bool ObjectLaneletFilterNode::isSameDirectionWithLanelets( if (object_velocity_norm < filter_settings_.lanelet_direction_filter_object_speed_threshold) { return true; } - for (const auto & lanelet : lanelets) { - const bool is_in_lanelet = - lanelet::utils::isInLanelet(object.kinematics.pose_with_covariance.pose, lanelet, 0.0); + + // we can not query by points, so create a small bounding box + // eps is not determined by any specific criteria; just a guess + constexpr double eps = 1.0e-6; + std::vector candidates; + const Point2d min_corner( + object.kinematics.pose_with_covariance.pose.position.x - eps, + object.kinematics.pose_with_covariance.pose.position.y - eps); + const Point2d max_corner( + object.kinematics.pose_with_covariance.pose.position.x + eps, + object.kinematics.pose_with_covariance.pose.position.y + eps); + const Box bbox(min_corner, max_corner); + + local_rtree.query(bgi::intersects(bbox), std::back_inserter(candidates)); + + for (const auto & box_and_lanelet : candidates) { + const bool is_in_lanelet = lanelet::utils::isInLanelet( + object.kinematics.pose_with_covariance.pose, box_and_lanelet.second, 0.0); if (!is_in_lanelet) { continue; } + const double lane_yaw = lanelet::utils::getLaneletAngle( - lanelet, object.kinematics.pose_with_covariance.pose.position); + box_and_lanelet.second, object.kinematics.pose_with_covariance.pose.position); const double delta_yaw = object_velocity_yaw - lane_yaw; const double normalized_delta_yaw = autoware::universe_utils::normalizeRadian(delta_yaw); const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw);