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(detected_object_validation): lanelet filter efficiency #1598

Closed
wants to merge 2 commits into from
Closed

perf(detected_object_validation): lanelet filter efficiency #1598

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
6f8edcd
perf PR 8109
technolojin Oct 23, 2024
f8daa46
perf PR 8240
technolojin Oct 23, 2024
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
31 changes: 21 additions & 10 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 @@ -25,12 +25,17 @@
#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_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 @@ -39,6 +44,13 @@ using tier4_autoware_utils::MultiPoint2d;
using tier4_autoware_utils::Point2d;
using tier4_autoware_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 All @@ -54,8 +66,6 @@ class ObjectLaneletFilterNode : public rclcpp::Node
std::unique_ptr<tier4_autoware_utils::DebugPublisher> debug_publisher_{nullptr};

lanelet::LaneletMapPtr lanelet_map_ptr_;
lanelet::ConstLanelets road_lanelets_;
lanelet::ConstLanelets shoulder_lanelets_;
std::string lanelet_frame_id_;

tf2_ros::Buffer tf_buffer_;
Expand All @@ -73,19 +83,20 @@ class ObjectLaneletFilterNode : public rclcpp::Node
bool filterObject(
const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
const lanelet::ConstLanelets & intersected_road_lanelets,
const lanelet::ConstLanelets & intersected_shoulder_lanelets,
const bg::index::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree,
autoware_auto_perception_msgs::msg::DetectedObjects & output_object_msg);
LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
lanelet::ConstLanelets getIntersectedLanelets(
const LinearRing2d &, const lanelet::ConstLanelets &);
LinearRing2d getConvexHullFromObjectFootprint(
const autoware_auto_perception_msgs::msg::DetectedObject & object);
std::vector<BoxAndLanelet> getIntersectedLanelets(const LinearRing2d &);
bool isObjectOverlapLanelets(
const autoware_auto_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_auto_perception_msgs::msg::DetectedObject & object);
const autoware_auto_perception_msgs::msg::DetectedObject & object,
const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree);
geometry_msgs::msg::Polygon setFootprint(
const autoware_auto_perception_msgs::msg::DetectedObject &);
};
Expand Down
183 changes: 133 additions & 50 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,7 +22,11 @@
#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>

namespace object_lanelet_filter
Expand Down Expand Up @@ -72,9 +76,6 @@ void ObjectLaneletFilterNode::mapCallback(
lanelet_frame_id_ = map_msg->header.frame_id;
lanelet_map_ptr_ = std::make_shared<lanelet::LaneletMap>();
lanelet::utils::conversion::fromBinMsg(*map_msg, lanelet_map_ptr_);
const lanelet::ConstLanelets all_lanelets = lanelet::utils::query::laneletLayer(lanelet_map_ptr_);
road_lanelets_ = lanelet::utils::query::roadLanelets(all_lanelets);
shoulder_lanelets_ = lanelet::utils::query::shoulderLanelets(all_lanelets);
}

void ObjectLaneletFilterNode::objectCallback(
Expand All @@ -97,22 +98,27 @@ void ObjectLaneletFilterNode::objectCallback(
return;
}

// calculate convex hull
const auto convex_hull = getConvexHull(transformed_objects);
// get intersected lanelets
lanelet::ConstLanelets intersected_road_lanelets =
getIntersectedLanelets(convex_hull, road_lanelets_);
lanelet::ConstLanelets intersected_shoulder_lanelets =
getIntersectedLanelets(convex_hull, shoulder_lanelets_);

// 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_road_lanelets, intersected_shoulder_lanelets,
output_object_msg);
if (!transformed_objects.objects.empty()) {
// calculate convex hull
const auto convex_hull = getConvexHull(transformed_objects);

// get intersected lanelets
std::vector<BoxAndLanelet> intersected_lanelets_with_bbox = getIntersectedLanelets(convex_hull);

// 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);

// Publish debug info
Expand All @@ -128,18 +134,20 @@ void ObjectLaneletFilterNode::objectCallback(
bool ObjectLaneletFilterNode::filterObject(
const autoware_auto_perception_msgs::msg::DetectedObject & transformed_object,
const autoware_auto_perception_msgs::msg::DetectedObject & input_object,
const lanelet::ConstLanelets & intersected_road_lanelets,
const lanelet::ConstLanelets & intersected_shoulder_lanelets,
const bgi::rtree<BoxAndLanelet, RtreeAlgo> & local_rtree,
autoware_auto_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_road_lanelets) ||
isObjectOverlapLanelets(transformed_object, intersected_shoulder_lanelets);
const bool is_polygon_overlap = isObjectOverlapLanelets(transformed_object, local_rtree);
filter_pass = filter_pass && is_polygon_overlap;
}

Expand All @@ -148,9 +156,7 @@ bool ObjectLaneletFilterNode::filterObject(
transformed_object.kinematics.orientation_availability ==
autoware_auto_perception_msgs::msg::TrackedObjectKinematics::UNAVAILABLE;
if (filter_settings_.lanelet_direction_filter && !orientation_not_available) {
const bool is_same_direction =
isSameDirectionWithLanelets(intersected_road_lanelets, transformed_object) ||
isSameDirectionWithLanelets(intersected_shoulder_lanelets, transformed_object);
const bool is_same_direction = isSameDirectionWithLanelets(transformed_object, local_rtree);
filter_pass = filter_pass && is_same_direction;
}

Expand Down Expand Up @@ -204,31 +210,74 @@ LinearRing2d ObjectLaneletFilterNode::getConvexHull(
}

LinearRing2d convex_hull;
boost::geometry::convex_hull(candidate_points, convex_hull);
bg::convex_hull(candidate_points, convex_hull);

return convex_hull;
}

lanelet::ConstLanelets ObjectLaneletFilterNode::getIntersectedLanelets(
const LinearRing2d & convex_hull, const lanelet::ConstLanelets & road_lanelets)
LinearRing2d ObjectLaneletFilterNode::getConvexHullFromObjectFootprint(
const autoware_auto_perception_msgs::msg::DetectedObject & object)
{
lanelet::ConstLanelets intersected_lanelets;
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;
bg::convex_hull(candidate_points, convex_hull);

return convex_hull;
}

// WARNING: This implementation currently iterate all lanelets, which could degrade performance
// when handling large sized map.
for (const auto & road_lanelet : road_lanelets) {
if (boost::geometry::intersects(convex_hull, road_lanelet.polygon2d().basicPolygon())) {
intersected_lanelets.emplace_back(road_lanelet);
// fetch the intersected candidate lanelets with bounding box and then
// check the intersections among the lanelets and the convex hull
std::vector<BoxAndLanelet> ObjectLaneletFilterNode::getIntersectedLanelets(
const LinearRing2d & convex_hull)
{
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>> bbox_of_convex_hull;
bg::envelope(convex_hull, bbox_of_convex_hull);
const lanelet::BoundingBox2d bbox2d(
lanelet::BasicPoint2d(
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>(bbox_of_convex_hull),
bg::get<bg::max_corner, 1>(bbox_of_convex_hull)));

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 (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_auto_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 (object.shape.type != autoware_auto_perception_msgs::msg::Shape::POLYGON) {
Polygon2d polygon;
const auto footprint = setFootprint(object);
Expand All @@ -238,39 +287,57 @@ 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 =
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)) {

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_auto_perception_msgs::msg::DetectedObject & object)
const autoware_auto_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 @@ -284,14 +351,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 = tier4_autoware_utils::normalizeRadian(delta_yaw);
const double abs_norm_delta_yaw = std::fabs(normalized_delta_yaw);
Expand Down
Loading