Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

perf(autoware_detected_object_validation): reduce lanelet_filter processing time (#8240) #1433

Merged
merged 1 commit into from
Jul 30, 2024
Merged

perf(autoware_detected_object_validation): reduce lanelet_filter processing time (#8240) #1433

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 21 additions & 6 deletions ...ation/include/detected_object_validation/detected_object_filter/object_lanelet_filter.hpp
View file
Open in desktop
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
Open in desktop
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
Loading