Skip to content

Commit

Permalink
perf(autoware_detected_object_validation): reduce lanelet_filter proc…
Browse files Browse the repository at this point in the history 
…essing time (autowarefoundation#8240) (#1433)

* add local r-tree for fast searching



change to _func__

add more debug

use local rtree

fix

tmp update

fix bug

clean unused

clean up



* clean up



* style(pre-commit): autofix

* chore: Optimize object filtering and improve performance

The code changes in `lanelet_filter.cpp` optimize the object filtering process by using the `empty()` function instead of checking the size of the `transformed_objects.objects` vector. This change improves performance and simplifies the code logic.

Refactor the code to use `empty()` instead of `size()` for checking if the `transformed_objects.objects` vector is empty. This change improves readability and performance.



---------

Signed-off-by: a-maumau <[email protected]>
Signed-off-by: Taekjin LEE <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
@technolojin @pre-commit-ci
technolojin and pre-commit-ci[bot] authored Jul 30, 2024
1 parent 622d6e3 commit a1e97cb
Show file tree
Hide file tree
Showing 2 changed files with 134 additions and 51 deletions.
27 changes: 21 additions & 6 deletions ...ation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 object_lanelet_filter
{
Expand All @@ -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<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:
Expand Down Expand Up @@ -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<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_;
Expand Down
158 changes: 113 additions & 45 deletions perception/detected_object_validation/src/object_lanelet_filter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,10 @@
#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>

Expand Down Expand Up @@ -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<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);

Expand All @@ -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<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;
}

Expand All @@ -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;
}

Expand Down Expand Up @@ -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<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);
Expand All @@ -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<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 =
Expand All @@ -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<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(
Expand All @@ -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<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);
Expand Down

0 comments on commit a1e97cb

Please sign in to comment.