perf(autoware_detected_object_validation): reduce lanelet_filter processing time

perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp

@@ -1,4 +1,4 @@
 // 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.
@@ -15,15 +15,18 @@
 #include "lanelet_filter.hpp"
 
 #include "autoware/universe_utils/geometry/geometry.hpp"
+#include "autoware/universe_utils/system/time_keeper.hpp"
 #include "autoware_lanelet2_extension/utility/message_conversion.hpp"
 #include "autoware_lanelet2_extension/utility/query.hpp"
 #include "object_recognition_utils/object_recognition_utils.hpp"
 
 #include <boost/geometry/algorithms/convex_hull.hpp>
 #include <boost/geometry/algorithms/disjoint.hpp>
 #include <boost/geometry/algorithms/intersects.hpp>
+#include <boost/geometry/index/rtree.hpp>
 
 #include <lanelet2_core/geometry/BoundingBox.h>
+#include <lanelet2_core/geometry/Lanelet.h>
 #include <lanelet2_core/geometry/LaneletMap.h>
 #include <lanelet2_core/geometry/Polygon.h>
 
@@ -100,18 +103,27 @@
     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<BoxAndLanelet> 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<BoxAndLanelet, RtreeAlgo> 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);
 
@@ -128,26 +140,29 @@
 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<BoxAndLanelet, RtreeAlgo> & 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;
     }
 
     // 2. check if objects velocity is the same with the lanelet direction
     const bool orientation_not_available =
       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;
     }
 
@@ -199,55 +214,75 @@
       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<BoxAndLanelet> ObjectLaneletFilterNode::getIntersectedLanelets(
   const LinearRing2d & convex_hull)
 {
-  namespace bg = boost::geometry;
-
-  lanelet::ConstLanelets intersected_lanelets;
+  std::vector<BoxAndLanelet> intersected_lanelets_with_bbox;
 
   // convert convex_hull to a 2D bounding box for searching in the LaneletMap
-  bg::model::box<bg::model::d2::point_xy<double>> bbox2d_convex_hull;
-  bg::envelope(convex_hull, bbox2d_convex_hull);
-  lanelet::BoundingBox2d bbox2d(
+  bg::model::box<bg::model::d2::point_xy<double>> bbox_of_convex_hull;
+  bg::envelope(convex_hull, bbox_of_convex_hull);
+  const lanelet::BoundingBox2d bbox2d(
     lanelet::BasicPoint2d(
-      bg::get<bg::min_corner, 0>(bbox2d_convex_hull),
-      bg::get<bg::min_corner, 1>(bbox2d_convex_hull)),
+      bg::get<bg::min_corner, 0>(bbox_of_convex_hull),
+      bg::get<bg::min_corner, 1>(bbox_of_convex_hull)),
     lanelet::BasicPoint2d(
-      bg::get<bg::max_corner, 0>(bbox2d_convex_hull),
-      bg::get<bg::max_corner, 1>(bbox2d_convex_hull)));
+      bg::get<bg::max_corner, 0>(bbox_of_convex_hull),
+      bg::get<bg::max_corner, 1>(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<BoxAndLanelet, RtreeAlgo> & 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);
@@ -258,8 +293,17 @@
       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<BoxAndLanelet> candidates;
+    bg::model::box<bg::model::d2::point_xy<double>> 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 =
@@ -268,30 +312,39 @@
       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<BoxAndLanelet, RtreeAlgo> & 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<BoxAndLanelet> candidates;
+  bg::model::box<bg::model::d2::point_xy<double>> 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<BoxAndLanelet, RtreeAlgo> & local_rtree)
 {
   const double object_yaw = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
   const double object_velocity_norm = std::hypot(
@@ -305,14 +358,30 @@
   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<BoxAndLanelet> 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);

perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp

@@ -26,12 +26,17 @@
 #include "autoware_map_msgs/msg/lanelet_map_bin.hpp"
 #include "autoware_perception_msgs/msg/detected_objects.hpp"
 
+#include <boost/geometry/index/rtree.hpp>
+
 #include <lanelet2_core/Forward.h>
+#include <lanelet2_core/geometry/Lanelet.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 namespace autoware::detected_object_validation
 {
@@ -42,6 +47,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<double, 2, bg::cs::cartesian>;
+using Box = boost::geometry::model::box<Point2d>;
+using BoxAndLanelet = std::pair<Box, lanelet::Lanelet>;
+using RtreeAlgo = bgi::rstar<16>;
+
 class ObjectLaneletFilterNode : public rclcpp::Node
 {
 public:
@@ -74,17 +86,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<BoxAndLanelet, RtreeAlgo> & 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<BoxAndLanelet> 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<BoxAndLanelet, RtreeAlgo> & local_rtree);
+  bool isPolygonOverlapLanelets(
+    const Polygon2d & polygon, const bgi::rtree<BoxAndLanelet, RtreeAlgo> & 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<BoxAndLanelet, RtreeAlgo> & local_rtree);
   geometry_msgs::msg::Polygon setFootprint(const autoware_perception_msgs::msg::DetectedObject &);
 
   std::unique_ptr<autoware::universe_utils::PublishedTimePublisher> published_time_publisher_;